Fixed definition locations > extracted to file

3 files changed, 150 insertions, 43 deletions

diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino
index a1a158c..5fe4610 100644
--- a/Venus_Skeleton/Venus_Skeleton.ino
+++ b/Venus_Skeleton/Venus_Skeleton.ino
@@ -17,13 +17,17 @@
 // 3		Error: Sensor IR Line detection
 
 #include <Servo.h>
-#include "dataTypes.h"
+#include <HMC5883L.h>
+#include <Wire.h>
 
 // **********************
 // ** Calibration file **
 // **********************
 
-// Include Wally of Eve, (un)comment the right one.
+#include "definitions.h"
+#include "dataTypes.h"
+
+// Include Wally or Eve, (un)comment the right one.
 #include "calibration_wall-e.h"
 //#include "calibration.eve.h"
 
@@ -32,22 +36,14 @@
 // ** DEFINITIONS **
 // *****************
 
-// Pin configuration
-#define PIN_SENS_COMPASS
-#define PIN_SENS_OBSTACLE_TURRET 9
-#define PIN_SENS_IR_LEFT A1
-#define PIN_SENS_IR_RIGHT A0
-#define PIN_SENS_SAMPLE_TURRET
-#define PIN_SENS_SAMPLE_GRIPPER
-#define PIN_SENS_BEACON_TURRET
-#define PIN_SERVO_LEFT 12
-#define PIN_SERVO_RIGHT 13
-#define PIN_SERVO_TURRET 11
-
 // Servos
 Servo servoLeft;
 Servo servoRight;
 Servo servoTurret;
+Servo servoGripper;
+
+// Compass
+HMC5883L compass;
 
 // Data variables
 data_t data;
@@ -63,11 +59,7 @@ int currValRobotY = 0;
 int LabX = 0;
 int LabY = 0;
 
-// Number of directions the robot can choose from: ((x-1) * (360/x)) degree. Preferably a multiple of 4, I think.
-#define NUM_DIRECTIONS 12
-
 // Number of directions the turret can measure from
-#define NUM_TURRET_DIRECTIONS 13
 int turretSequence[NUM_TURRET_DIRECTIONS] = {};
 
 // Sensor values
@@ -197,7 +189,57 @@ void readData() {
 // **********************
 
 // sensor Compass, orientation
+float sensMagnetometer() {
+	MagnetometerRaw raw = compass.ReadRawAxis();
+  float heading = atan2(raw.YAxis, raw.XAxis);
+
+  if(heading < 0){
+    heading += 2*PI;
+  }
+
+  float headingDegrees = heading * 180/M_PI;    // Convert to angles
+  headingDegrees = headingDegrees + 90;         // Adjust angle to be relative to front of robot instead of X-direction of sensor
+  if(headingDegrees>360){
+    headingDegrees=headingDegrees-360;
+  }
+  float north;
+    if(headingDegrees<165){                     // When heading is negative (left) of north with 15 degree margin
+    north=headingDegrees-180;                   // Set angle between north and heading angle
+  }
+  else if(headingDegrees>195){                  // When heading is positive (right) of north with 15 degree margin
+    north=headingDegrees-180;                   // Set angle between north and heading angle
+  }
+  else{                                         // Else heading is towards north with 15 degree margin both sides            
+    north=0;                                    // Set angle between north and heading angle to be 0
+  }
+  return(north);                                // Returns angle difference
+}
 int sensCompass() {
+	
+	
+	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);
+  }
+  return readingtotal;
+	
+	
+	
 	// average for last x ????????
   int degree = 359;
   sendData(PI_DATATYPE_COMPASS, degree);
@@ -244,11 +286,9 @@ void sensObstacleTurret() {
 // sensor IR, line detection
 bool sensIRLine(lr_t LorR) {
 	
-	
 	// BE AWARE, LOCALISATION OF LINE IS y DISTANCE and z DEGREE FROM CENTER OF ROBOT,
 	// calculate by RPi??
 	
-	
   int i;
 	bool inaccessible = false;
 	int pinNo = LorR == L ? PIN_SENS_IR_LEFT : PIN_SENS_IR_RIGHT;
@@ -270,20 +310,33 @@ bool sensIRLine(lr_t LorR) {
 }
 
 // sensor IR, sample detection
-int sensSampleTurret() {
-  boolean sample = false;
-  if(sample) {
-		// If a sample is detected, stop all servos, send data to the PI and wait for response
-		opMode(OPMODE_WAIT);
-    stopAllServos();
-    sendData(PI_DATATYPE_SAMPLETURRET, currValRobotX, currValRobotY, currValSensCompass, currValTurret);
+void sensSampleTurret() {
+	
+	// BE AWARE, LOCALISATION OF OBSTACLE IS y DISTANCE FROM CENTER OF ROBOT 
+	// (creates triangle with centerOfRobot, centerOfSensor and Obstacle as its corners)
+	
+	if (timerTurret == 0) {
+		int i;
+		bool sample = false;
+		
+		i = false; // SENSORDATA HERE
+		if (i > CAL_SAMPLE_TURRET_THRESHOLD) 
+			sample = true;
+		
+		if (data.sample_turret_detected[counterTurret] != sample) {
+			opMode(OPMODE_WAIT);
+	    stopAllServos();
+			data.sample_turret_detected[counterTurret] = sample;
+			// mark as changed
+			dataToPiChangedBits |= CHANGED_SENS_SAMPLETURRET;
+		}
   }
 }
-int sensSampleGripper() {
+void sensSampleGripper() {
   int i;
-	bool sample = false;
+	bool sample = false; 
 	
-	i = false; // sensor readout here
+	i = false; // SENSORDATA HERE
 	if (i > CAL_SAMPLE_GRIPPER_THRESHOLD) 
 		sample = true;
 	
@@ -292,15 +345,26 @@ int sensSampleGripper() {
 		// Mark as changed
 		dataToPiChangedBits |= CHANGED_SENS_SAMPLEGRIPPER;
 	}
-	
-  return sample;
 }
 
 // sensor IR, beacon detection and recognition
 int sensBeaconTurret() {
-  int beacon = 0;
-  if(beacon) {
-    // Location
+	// BE AWARE, LOCALISATION OF OBSTACLE IS y DISTANCE FROM CENTER OF ROBOT 
+	// (creates triangle with centerOfRobot, centerOfSensor and Obstacle as its corners)
+	
+	if (timerTurret == 0) {
+		int i;
+		bool beacon = false;
+		
+		i = false; // SENSORDATA HERE
+		if (i > CAL_SAMPLE_TURRET_THRESHOLD) 
+			beacon = true;
+		
+		if (data.beacon_detected[counterTurret] != sample) {
+			data.beacon_detected[counterTurret] = sample;
+			// mark as changed
+			dataToPiChangedBits |= CHANGED_SENS_SAMPLETURRET;
+		}
   }
 }
 
@@ -497,11 +561,26 @@ void setup() {
 	// Send some values:
 	// - Number of possible directions
 	
+	Wire.begin();
+	
 	initTurretSequence(turretSequence, NUM_TURRET_DIRECTIONS);
 	
-	servoLeft.attach(servoLeftPin);
-	servoRight.attach(servoRightPin);
-	servoTurret.attach(servoTurretPin);
+	// initialise compass communication
+	compass = HMC5883L();
+	// Gauss scale 1.3
+  int error = compass.SetScale(1.3);
+  if(error != 0){
+  	// Serial.println(compass.GetErrorText(error));
+  }
+  error = compass.SetMeasurementMode(Measurement_Continuous);
+  if(error != 0){
+  	// Serial.println(compass.GetErrorText(error));
+  }
+	
+	servoLeft.attach(PIN_SERVO_LEFT);
+	servoRight.attach(PIN_SERVO_RIGHT);
+	servoTurret.attach(PIN_SERVO_TURRET);
+  servoGrabber.attach(PIN_SERVO_GRIPPER);
 	
 	turnTurretTo(0);
 	
diff --git a/Venus_Skeleton/dataTypes.h b/Venus_Skeleton/dataTypes.h
index 9a77ae1..3829da1 100644
--- a/Venus_Skeleton/dataTypes.h
+++ b/Venus_Skeleton/dataTypes.h
@@ -10,15 +10,18 @@ typedef enum {
 
 // Sensor data
 typedef struct {
-	// ir
 	bool IR_left_detected;
 	bool IR_right_detected;
-	bool sample_turret_detected;
 	bool sample_gripper_detected;
-	int distance;
+	bool sample_turret_detected[NUM_TURRET_DIRECTIONS];
+	bool beacon_detected[NUM_TURRET_DIRECTIONS];
 	long obstacle_turret_distances[NUM_TURRET_DIRECTIONS];
-	// ..
-	long timer; // 32-bit
+	int robot_x[NUM_TURRET_DIRECTIONS];
+	int robot_y[NUM_TURRET_DIRECTIONS];
+	int robot_deg[NUM_TURRET_DIRECTIONS];
+	int robot_curr_x;
+	int robot_curr_y;
+	int robot_curr_deg;
 } data_t;
 enum {
 	CHANGED_SENS_COMPASS = 1 << 0,
diff --git a/Venus_Skeleton/definitions.h b/Venus_Skeleton/definitions.h
new file mode 100644
index 0000000..877dbdf
--- /dev/null
+++ b/Venus_Skeleton/definitions.h
@@ -0,0 +1,25 @@
+// *****************
+// ** DEFINITIONS **
+// *****************
+
+// Pin configuration
+#define PIN_SENS_COMPASS
+#define PIN_SENS_OBSTACLE_TURRET 9
+#define PIN_SENS_IR_LEFT A1
+#define PIN_SENS_IR_RIGHT A0
+#define PIN_SENS_SAMPLE_TURRET
+#define PIN_SENS_SAMPLE_GRIPPER
+#define PIN_SENS_BEACON_TURRET
+#define PIN_SERVO_LEFT 12
+#define PIN_SERVO_RIGHT 13
+#define PIN_SERVO_TURRET 11
+#define PIN_SERVO_GRIPPER 10
+
+// Number of directions the robot can choose from: ((x-1) * (360/x)) degree. Preferably a multiple of 4, I think.
+#define NUM_DIRECTIONS 12
+
+// Number of directions the turret can measure from
+#define NUM_TURRET_DIRECTIONS 13
+
+// Number of compass checks to create average
+#define NUM_COMPASS_CHECKS 12
\ No newline at end of file