4th - with example of bitwise sensor changes

3 files changed, 147 insertions, 36 deletions

diff --git a/RPi_skeleton/RPi_skeleton.xcodeproj/project.xcworkspace/xcuserdata/peterdekok.xcuserdatad/UserInterfaceState.xcuserstate b/RPi_skeleton/RPi_skeleton.xcodeproj/project.xcworkspace/xcuserdata/peterdekok.xcuserdatad/UserInterfaceState.xcuserstate
index 09ca94e..91ef072 100644
--- a/RPi_skeleton/RPi_skeleton.xcodeproj/project.xcworkspace/xcuserdata/peterdekok.xcuserdatad/UserInterfaceState.xcuserstate
+++ b/RPi_skeleton/RPi_skeleton.xcodeproj/project.xcworkspace/xcuserdata/peterdekok.xcuserdatad/UserInterfaceState.xcuserstate
diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino
index f4cc57b..6b901c8 100644
--- a/Venus_Skeleton/Venus_Skeleton.ino
+++ b/Venus_Skeleton/Venus_Skeleton.ino
@@ -3,47 +3,75 @@
 // - limit sendData to x per second
 //
 // - insert into calibration files
-// 		const int calObstacleTurretMaxDist = 50;
-// 		const int calObstacleTurretMinDist = 10;
-// 		const int calIRLeftThreshold = 2;
-// 		const int calIRRightThreshold = 2;
-// 		#define OPERATION_MAX_CHANGE_TIME = 20000;
+// 		
 // 
 // - Make sure ENUM piDataType is correct and synced
 // - Change interpretData to resemble correct ENUM data
 // 
 
+#include <Servo.h>
+
 // **********************
 // ** Calibration file **
 // **********************
 
 // Include Wally of Eve, (un)comment the right one.
-#include "calibration.wally.h"
+#include "calibration.wall-e.h"
 //#include "calibration.eve.h"
 
+
 // *****************
 // ** DEFINITIONS **
 // *****************
 
 // Pin configuration
-#define PIN_SENS_COMPASS;
-#define PIN_SENS_OBSTACLE_TURRET = 9;
-#define PIN_SENS_IR_LEFT;
-#define PIN_SENS_IR_RIGHT;
-#define PIN_SENS_SAMPLE_TURRET;
-#define PIN_SENS_SAMPLE_GRIPPER;
-#define PIN_SENS_BEACON_TURRET;
-#define PIN_SERVO_LEFT = 12;
+#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;	// Declare left servo signal
+Servo servoRight;	// Declare right servo signal
+
+
 // Raspberry Pi Data type declarations
-enum piDataType {COMPASS, OBSTACLETURRET, IRLEFT, IRRIGHT, SAMPLETURRET, SAMPLEGRIPPER, BEACONTURRET, LAB, MOVETO, GRIPPER, TURRET};
+typedef enum {
+	PI_DATATYPE_COMPASS, 
+	PI_DATATYPE_OBSTACLETURRET, 
+	PI_DATATYPE_IRLEFT, 
+	PI_DATATYPE_IRRIGHT, 
+	PI_DATATYPE_SAMPLETURRET, 
+	PI_DATATYPE_SAMPLEGRIPPER, 
+	PI_DATATYPE_BEACONTURRET, 
+	PI_DATATYPE_LAB, 
+	PI_DATATYPE_MOVETO, 
+	PI_DATATYPE_GRIPPER, 
+	PI_DATATYPE_TURRET
+} pi_datatype_t;
 // Operation modes
-enum opMode {RANDOM, WAIT, INITIALSEQUENCE, RANDOM, MAPPING, GOTOSAMPLE, GRABSAMPLE, GOTOLABLOCATION, WAITFORLAB, FINDMAGNET, LABSEQUENCE};
+typedef enum {
+	OPMODE_RANDOM, 
+	OPMODE_WAIT, 
+	OPMODE_INITIALSEQUENCE, 
+	OPMODE_RANDOM, 
+	OPMODE_MAPPING, 
+	OPMODE_GOTOSAMPLE, 
+	OPMODE_GRABSAMPLE, 
+	OPMODE_GOTOLABLOCATION, 
+	OPMODE_WAITFORLAB, 
+	OPMODE_FINDMAGNET, 
+	OPMODE_LABSEQUENCE
+} opmode_t;
 
 // Current operation mode
-opMode operationMode = 0;
+opmode_t operationMode = OPMODE_WAIT;
 unsigned long operationChange = 0;
 
 // Location values
@@ -64,7 +92,7 @@ int currValTurret;
 // **********************
 
 // sendData to Raspberry Pi
-void sendData(int method, int data, int data1 = null, int data2 = null) {
+void sendData(pi_datatype_t method, int data, int data1 = null, int data2 = null) {
   
 }
 
@@ -74,21 +102,21 @@ void readData() {
   
 	// if data received -> interpretate it
 	// change operation mode + update timer
-	operationMode = WAIT;
+	operationMode = OPMODE_WAIT;
 	operationChange = millis();
 	// for example:
-	interpretData(MOVETO, 0);
+	interpretData(PI_DATATYPE_MOVETO, 0);
 }
 
-void interpretData(piDataType dataType, int message) {
+void interpretData(pi_datatype_t dataType, int message) {
   switch(dataType) {
-    case MOVETO:
+    case PI_DATATYPE_MOVETO:
 		
     break;
-    case GRIPPER:
+    case PI_DATATYPE_GRIPPER:
     
     break;
-    case TURRET:
+    case PI_DATATYPE_TURRET:
     
     break;
     case default:
@@ -106,7 +134,7 @@ void interpretData(piDataType dataType, int message) {
 int sensCompass() {
 	// average for last x ????????
   int degree = 359;
-  sendData(COMPASS, degree);
+  sendData(PI_DATATYPE_COMPASS, degree);
   return degree;
 }
 
@@ -114,26 +142,36 @@ int sensCompass() {
 int sensObstacleTurret() {
   int distance = 100;
   int turretAngle = 90;
-  sendData(OBSTACLETURRET, distance, turretAngle);
+  sendData(PI_DATATYPE_OBSTACLETURRET, distance, turretAngle);
   return distance;
 }
 
 // sensor IR, line detection
 boolean sensIRLeft() {
+  int i;
+	boolean inaccessible = false;
+  i = analogRead(PIN_SENS_IR_LEFT);
+  if (i > CAL_IR_LEFT_THRESHOLD)
+    inaccessible = true;
+	
   // Inaccessible terrain. True for inaccessible, false for accessible
-  boolean inaccessible = false;
   if(inaccessible) {
     stopMovement();
-    sendData(IRLEFT, currValRobotX, currValRobotY, currValSensCompass);
+    sendData(PI_DATATYPE_IRLEFT, currValRobotX, currValRobotY, currValSensCompass);
   }
   return inaccessible;
 }
 int sensIRRight() {
+  int i;
+	boolean inaccessible = false;
+  i = analogRead(PIN_SENS_IR_RIGHT);
+  if (i > CAL_IR_RIGHT_THRESHOLD)
+    inaccessible = true;
+	
   // Inaccessible terrain. True for inaccessible, false for accessible
-  boolean inaccessible = false;
   if(inaccessible) {
     stopMovement();
-    sendData(IRRIGHT, currValRobotX, currValRobotY, currValSensCompass);
+    sendData(PI_DATATYPE_IRRIGHT, currValRobotX, currValRobotY, currValSensCompass);
   }
   return inaccessible;
 }
@@ -144,14 +182,14 @@ int sensSampleTurret() {
   if(sample) {
 		// If a sample is detected, stop all servos, send data to the PI and wait for response
     stopAllServos();
-    sendData(SAMPLETURRET, currValRobotX, currValRobotY, currValSensCompass, currValTurret);
+    sendData(PI_DATATYPE_SAMPLETURRET, currValRobotX, currValRobotY, currValSensCompass, currValTurret);
   }
 }
 int sensSampleGripper() {
   boolean sample = false;
   if(sample) {
     stopAllServos();
-    sendData(SAMPLEGRIPPER, currValRobotX, currValRobotY, currValSensCompass);
+    sendData(PI_DATATYPE_SAMPLEGRIPPER, currValRobotX, currValRobotY, currValSensCompass);
   }
 }
 
@@ -173,10 +211,14 @@ int sensLab() {
 // ************************
 
 void stopMovement() {
-  
+	operationMode = OPMODE_WAIT;
+	servoLeft.detach();
+	servoRight.detach();
 }
 void stopAllServos() {
-  
+	operationMode = OPMODE_WAIT;
+	servoLeft.detach();
+	servoRight.detach();
 }
 
 // ***********************
@@ -197,8 +239,14 @@ void loop() {
 	sensBeaconTurret();
 	sensLab();
 	
-	if((operationChange + OPERATION_MAX_CHANGE_TIME) > millis() && operationMode != RANDOM) {
-		operationMode = RANDOM;
+	switch(operationMode) {
+		case OPMODE_WAIT:
+		stopMovement();
+		break;
+		
+		default:
+		stopMovement();
+		break;
 	}
 	
 }
@@ -224,3 +272,60 @@ long microsecondsToCentimeters(long microseconds)
   // object we take half of the distance travelled.
   return microseconds / 29 / 2;
 }
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+// define types
+typedef struct {
+	// ir
+	bool left_detected;
+	bool right_detected; // usually an integer (16-bit)
+	int distance; // 16-bit
+	// ..
+	long timer; // 32-bit
+} sensor_data_t;
+
+enum {
+	CHANGED_IR_LEFT = 1 << 0,
+	CHANGED_IR_RIGHT = 1 << 1,
+	CHNAGED_JDHCHDUCDH = 1 << 2,
+		CHANGED_IDJDHD = 1 << 3
+};
+
+sensor_data_t data;
+int changedBits = 0;
+
+// 1. gather sensor data -< sets changedBits and data
+val = readFromSensor();
+if (data.left_detected != val) {
+	data.left_detected = val;
+	// mark as changed
+	changedBits |= CHANGED_IR_LEFT;
+}
+// other sensors: same
+
+// 2. read whetehr you have hit a block / line sensor
+if ((changedBits & (CHANGED_IR_LEFT | CHANGED_IR_RIGHT))) &&
+	(data.left_detected || data.right_detected)) {
+		// panic!
+		stopMotor();
+}
+
+// 3. send to pi or other calculations (and clear bits)
+sendData(PIN_IR_LEFT, data.left_detected);
+// clear bits
+changedBits &= ~CHANGED_IR_LEFT;
\ No newline at end of file
diff --git a/Venus_Skeleton/calibration.wall-e.h b/Venus_Skeleton/calibration.wall-e.h
new file mode 100644
index 0000000..e667d56
--- /dev/null
+++ b/Venus_Skeleton/calibration.wall-e.h
@@ -0,0 +1,6 @@
+// WALLY configuration file
+
+#define calObstacleTurretMaxDist 50;
+#define calObstacleTurretMinDist 10;
+#define CAL_IR_LEFT_THRESHOLD 600
+#define CAL_IR_RIGHT_THRESHOLD 600
\ No newline at end of file