Testing now

3 files changed, 118 insertions, 53 deletions

diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino
index e07a2b4..d784255 100644
--- a/Venus_Skeleton/Venus_Skeleton.ino
+++ b/Venus_Skeleton/Venus_Skeleton.ino
@@ -16,6 +16,14 @@
 // 2		Error: Sensor Obstacle detection on the turret
 // 3		Error: Sensor IR Line detection
 
+#define DEBUG true
+
+#ifdef DEBUG
+#define DEBUG_PRINT(str) Serial.print(str)
+#else
+#define DEBUG_PRINT(str) do { } while (0)
+#endif
+
 #include <Servo.h>
 #include <HMC5883L.h>
 #include <Wire.h>
@@ -28,6 +36,7 @@
 #include "dataTypes.h"
 
 // Include Wally or Eve, (un)comment the right one.
+#define ROVER "wall-e"
 #include "calibration_wall-e.h"
 //#include "calibration.eve.h"
 
@@ -79,6 +88,7 @@ long timerTurret = 0;
 // Counters
 int counterMovement = 0;
 int counterTurret = -1;
+int initCounter = 0;
 
 
 // **********************
@@ -124,7 +134,7 @@ long microsecondsToCentimeters(long microseconds)
 	return microseconds / 29 / 2;
 }
 
-void initTurretSequence(int array, int numOfPositions) {
+void initTurretSequence(int array[], int numOfPositions) {
 	int sign = 1;
 	int x = 0;
 
@@ -150,7 +160,7 @@ void initTurretSequence(int array, int numOfPositions) {
 // **********************
 
 // sendData to Raspberry Pi
-void sendData(pi_datatype_t method, int data, int data1 = NULL, int data2 = NULL, int data3 = NULL) {
+void sendData(pi_datatype_t method, int data, int data1, int data2, int data3) {
   
 }
 
@@ -214,36 +224,25 @@ float sensMagnetometer() {
 	}
 	return(north);                                // Returns angle difference
 }
-int sensCompass() {
-	
-	
+void sensCompass() {
+	int degree = -1;
 	float reading[NUM_COMPASS_CHECKS];
 	float readingtotal = 0;
 	
 	for(int i = 0; i < NUM_COMPASS_CHECKS; i++){
-		reading[i] = readcompass();
-		readingtotal
-			i++;
-		reading[i] = readcompass();
-		i++;
-		reading[i] = readcompass();
-	}
-	if(i==NUM_COMPASS_CHECKS){
-		for(int j = 0; j < NUM_COMPASS_CHECKS; j++){
-			readingtotal = readingtotal + reading[j];     // Total readings
-		}
-		readingtotal = readingtotal / NUM_COMPASS_CHECKS;                 // Average reading
-		Serial.print("Readingtotal: ");               // Print average reading (CAN BE REMOVED)
-		Serial.println(readingtotal);
+		readingtotal += sensMagnetometer();
 	}
-	return readingtotal;
-	
 	
+	readingtotal /= NUM_COMPASS_CHECKS;
+  
+	if (readingtotal >= 0 && readingtotal <= 360)
+		degree = (int) readingtotal;
 	
-	// average for last x ????????
-	int degree = 359;
-	sendData(PI_DATATYPE_COMPASS, degree);
-	return degree;
+	if (data.compass != degree) {
+		data.compass = degree;
+		// mark as changed
+		dataToPiChangedBits |= CHANGED_SENS_COMPASS;
+	}
 }
 
 // sensor Ultrasound, distance measurement
@@ -284,7 +283,7 @@ void sensObstacleTurret() {
 
 
 // sensor IR, line detection
-bool sensIRLine(lr_t LorR) {
+void sensIRLine(lr_t LorR) {
 	
 	// BE AWARE, LOCALISATION OF LINE IS y DISTANCE and z DEGREE FROM CENTER OF ROBOT,
 	// calculate by RPi??
@@ -305,8 +304,6 @@ bool sensIRLine(lr_t LorR) {
 		// mark as changed
 		dataToPiChangedBits |= changeBit;
 	}
-	
-	return inaccessible;
 }
 
 // sensor IR, sample detection
@@ -360,8 +357,8 @@ int sensBeaconTurret() {
 		if (i > CAL_SAMPLE_TURRET_THRESHOLD) 
 			beacon = true;
 		
-		if (data.beacon_detected[counterTurret] != sample) {
-			data.beacon_detected[counterTurret] = sample;
+		if (data.beacon_detected[counterTurret] != beacon) {
+			data.beacon_detected[counterTurret] = beacon;
 			// mark as changed
 			dataToPiChangedBits |= CHANGED_SENS_SAMPLETURRET;
 		}
@@ -369,23 +366,31 @@ int sensBeaconTurret() {
 }
 
 // sensor IR, lab detection
-int sensLab() {
-	return 0;
+void sensLab() {
+	// ...
 }
 
 // ************************
 // ** ACTUATOR FUNCTIONS **
 // ************************
 
+void maneuver(int speedLeft, int speedRight){
+  speedLeft *= CAL_SERVO_LEFT;
+  speedRight *= CAL_SERVO_RIGHT;
+	
+  servoLeft.writeMicroseconds(1500 + speedLeft);         // Set left servo speed
+  servoRight.writeMicroseconds(1500 - speedRight);       // Set right servo speed
+}
+
 void stopMovement() {
-	servoLeft.detach();
-	servoRight.detach();
+	DEBUG_PRINT(" Stop Movement\n");
+	maneuver(0, 0);
+	timerTurret == 0;
 }
 
 void stopAllServos() {
-	servoLeft.detach();
-	servoRight.detach();
-	
+	DEBUG_PRINT(" Stop All Servos\n");
+	maneuver(0, 0); 
 	timerTurret == 0;
 }
 
@@ -399,15 +404,26 @@ bool moveTurnTo(int direction) {
 	// if movementtimer is expired, stop movement.
 	
 	if (!timerMovement) {
+		DEBUG_PRINT("\n  Start turn move: ");
+		DEBUG_PRINT(direction);
+		DEBUG_PRINT(" - ");
 		counterMovement++;
 		// timer = now() + direction * formula
-		timerMovement = millis() + (direction % NUM_DIRECTIONS) * 200;
-		
+		timerMovement = millis() + (direction % NUM_DIRECTIONS) * CAL_MOVE_TURN * (360 / NUM_DIRECTIONS);
 		
 		// DO A COMPASS CHECK
-		// movePivot('R', somespeed);
-		
 		
+		maneuver(0,0);
+		if(direction < NUM_DIRECTIONS){
+			DEBUG_PRINT(" left\n\n");
+			// turn left
+			maneuver(-100,100);
+		}
+		if(direction >= NUM_DIRECTIONS){
+			DEBUG_PRINT(" right\n\n");
+			// Turn right
+			maneuver(100,-100);
+		}
 	}
 	if (timerMovement < millis()) {
 		stopMovement();
@@ -415,12 +431,37 @@ bool moveTurnTo(int direction) {
 	}
 }
 
-void moveStraight(int direction, int distance) {
+void moveStraight(int direction, float distance) {
 	if (direction != 0 && direction != NUM_DIRECTIONS) {
 		return;
 	}
 	
+	// Convert distance into drive time
+	float drivetime = distance / 0.0154;
+	
+	if (!timerMovement) {
+		DEBUG_PRINT("\n  Start straight move: ");
+		DEBUG_PRINT(distance);
+		DEBUG_PRINT(" - ");
+		counterMovement++;
+		// timer = now() + direction * formula
+		timerMovement = millis() + drivetime;
+		
+		// If input is positive drive forward
+		if(distance > 0) {
+			DEBUG_PRINT(" fw\n\n");
+			maneuver(100, 100);
+		}
+		// If input is negative drive backwards
+		if(distance < 0){
+			DEBUG_PRINT(" bw\n\n");
+			maneuver(-100, -100);
+		}
+	}
 	
+	if (timerMovement < millis()) {
+		stopMovement();
+	}
 }
 
 void turnTurretTo(int deg) {
@@ -438,7 +479,7 @@ void turnTurretToNext() {
 		deg = turretSequence[counterTurret] * 10;
 		
 		timerTurret = millis();
-		turnTurretTo(deg)
+		turnTurretTo(deg);
 	}
 	if ((timerTurret + 100) < millis()) {
 		timerTurret = 0;
@@ -451,6 +492,9 @@ void turnTurretToNext() {
 // *********************
 
 void opMode(opmode_t opmode) {
+	DEBUG_PRINT("\nOPMODE");
+	DEBUG_PRINT(opmode);
+	DEBUG_PRINT("\n\n");
 	operationMode = opmode;
 }
 
@@ -459,6 +503,7 @@ void checkFreePath() {
 		// Left or Right IR sensor sees inaccessible terrain
 		opMode(OPMODE_WAIT);
 		stopMovement();
+		DEBUG_PRINT(" NO FREE PATH!! ");
 	}
 	
 	// Ultrasound
@@ -562,6 +607,13 @@ void setup() {
 	// - Number of possible directions
 	
 	Wire.begin();
+#ifdef DEBUG
+	Serial.begin(9600);
+#endif
+	
+	DEBUG_PRINT("\n\n\n** ROVER ");
+	DEBUG_PRINT(ROVER);
+	DEBUG_PRINT(" **\n\n");
 	
 	initTurretSequence(turretSequence, NUM_TURRET_DIRECTIONS);
 	
@@ -570,17 +622,18 @@ void setup() {
 	// Gauss scale 1.3
 	int error = compass.SetScale(1.3);
 	if(error != 0){
-		// Serial.println(compass.GetErrorText(error));
+		DEBUG_PRINT(compass.GetErrorText(error));
+		DEBUG_PRINT("\n");
 	}
 	error = compass.SetMeasurementMode(Measurement_Continuous);
 	if(error != 0){
-		// Serial.println(compass.GetErrorText(error));
+		DEBUG_PRINT(compass.GetErrorText(error));
 	}
 	
 	servoLeft.attach(PIN_SERVO_LEFT);
 	servoRight.attach(PIN_SERVO_RIGHT);
 	servoTurret.attach(PIN_SERVO_TURRET);
-	servoGrabber.attach(PIN_SERVO_GRIPPER);
+	servoGripper.attach(PIN_SERVO_GRIPPER);
 	
 	turnTurretTo(0);
 	
@@ -588,6 +641,7 @@ void setup() {
 }
 
 void loop() {
+	
 	sensCompass();
 	sensObstacleTurret();
 	sensIRLine(L);
@@ -596,15 +650,15 @@ void loop() {
 	sensSampleGripper();
 	
 	sensLab();
-
-	checkSample();
+	
+	//checkSample();
 	checkFreePath();
 	
 	// calculateLocation();
 	
-	sendData(PI_DATATYPE_ALL, 'lala', 'lala');
+	//sendData(PI_DATATYPE_ALL, 'lala', 'lala');
 	dataToPiChangedBits = 0; // Clear changed bits
-	readData();
+	//readData();
 	
 	switch(operationMode) {
 		case OPMODE_WAIT:
@@ -621,7 +675,7 @@ void loop() {
 		turnTurretToNext();
 		checkBestRoute();
 		moveTurnTo(currValDirection);
-		moveStraight(currValDirection, 1); // 1 is supposed to be some distance / time
+		moveStraight(currValDirection, 20.0); // 1.0 is supposed to be some distance
 		break;
 		case OPMODE_CHECKSAMPLE:
 			
@@ -646,6 +700,11 @@ void loop() {
 		break;
 	}
 	
+	if (!initCounter) {
+		opMode(OPMODE_MAPPING);
+		initCounter++;
+	}
+	
 	// Delay for stability.
 	delay(20);
 }
\ No newline at end of file
diff --git a/Venus_Skeleton/calibration_wall-e.h b/Venus_Skeleton/calibration_wall-e.h
index 462fa1b..daf8a8e 100644
--- a/Venus_Skeleton/calibration_wall-e.h
+++ b/Venus_Skeleton/calibration_wall-e.h
@@ -5,5 +5,10 @@
 #define CAL_IR_LEFT_THRESHOLD 600
 #define CAL_IR_RIGHT_THRESHOLD 600
 #define CAL_SAMPLE_GRIPPER_THRESHOLD x
-#define CAL_INITIAL_DISTANCE x
-#define CAL_TURRET_DEGREE 7
\ No newline at end of file
+#define CAL_INITIAL_DISTANCE 30.0
+#define CAL_TURRET_DEGREE 7
+
+// Left wheel calibration factor   WALL-E: 1.4 EVE: 1.0
+#define CAL_SERVO_LEFT 1.4
+// Right wheel calibration factor  WALL-E: 1.0 EVE: 1.1
+#define CAL_SERVO_RIGHT 1.0	
\ No newline at end of file
diff --git a/Venus_Skeleton/dataTypes.h b/Venus_Skeleton/dataTypes.h
index 3829da1..9021dc6 100644
--- a/Venus_Skeleton/dataTypes.h
+++ b/Venus_Skeleton/dataTypes.h
@@ -16,6 +16,7 @@ typedef struct {
 	bool sample_turret_detected[NUM_TURRET_DIRECTIONS];
 	bool beacon_detected[NUM_TURRET_DIRECTIONS];
 	long obstacle_turret_distances[NUM_TURRET_DIRECTIONS];
+	int compass;
 	int robot_x[NUM_TURRET_DIRECTIONS];
 	int robot_y[NUM_TURRET_DIRECTIONS];
 	int robot_deg[NUM_TURRET_DIRECTIONS];