Commit 3

1 files changed, 88 insertions, 24 deletions

diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino
index 04bb5d3..f4cc57b 100644
--- a/Venus_Skeleton/Venus_Skeleton.ino
+++ b/Venus_Skeleton/Venus_Skeleton.ino
@@ -1,23 +1,50 @@
 // TODO
 //
 // - 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
+// 
 
+// **********************
+// ** Calibration file **
+// **********************
 
-
+// Include Wally of Eve, (un)comment the right one.
+#include "calibration.wally.h"
+//#include "calibration.eve.h"
 
 // *****************
 // ** DEFINITIONS **
 // *****************
 
 // Pin configuration
-#define pinSensCompass
-#define pinSensObstacleTurret
-#define pinSensIRLeft
-#define pinSensIRRight
-#define pinSensSampleTurret
-#define pinSensSampleGripper
-#define pinSensBeaconTurret
-#define pinSensLab
+#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_SERVO_RIGHT = 13;
+#define PIN_SERVO_TURRET = 11;
+
+// Raspberry Pi Data type declarations
+enum piDataType {COMPASS, OBSTACLETURRET, IRLEFT, IRRIGHT, SAMPLETURRET, SAMPLEGRIPPER, BEACONTURRET, LAB, MOVETO, GRIPPER, TURRET};
+// Operation modes
+enum opMode {RANDOM, WAIT, INITIALSEQUENCE, RANDOM, MAPPING, GOTOSAMPLE, GRABSAMPLE, GOTOLABLOCATION, WAITFORLAB, FINDMAGNET, LABSEQUENCE};
+
+// Current operation mode
+opMode operationMode = 0;
+unsigned long operationChange = 0;
 
 // Location values
 int currValRobotX;
@@ -31,16 +58,6 @@ int currValServoTurret;
 // Actuator values
 int currValTurret;
 
-// Calibration values
-#define calObstacleTurretMaxDist 50
-#define calObstacleTurretMinDist 10
-#define calIRLeftThreshold 2
-#define calIRRightThreshold 2
-#define cal
-
-// RPi Data type declarations
-enum piDataType {COMPASS, OBSTACLETURRET, IRLEFT, IRRIGHT, SAMPLETURRET, SAMPLEGRIPPER, BEACONTURRET, LAB};
-
 
 // **********************
 // ** PI COMMUNICATION **
@@ -51,20 +68,31 @@ void sendData(int method, int data, int data1 = null, int data2 = null) {
   
 }
 
+// receive data from Raspberry Pi
+// sequence = ... ?????
 void readData() {
   
+	// if data received -> interpretate it
+	// change operation mode + update timer
+	operationMode = WAIT;
+	operationChange = millis();
+	// for example:
+	interpretData(MOVETO, 0);
 }
 
-void interpretData(piDataType dataType, message) {
+void interpretData(piDataType dataType, int message) {
   switch(dataType) {
-    case x:
+    case MOVETO:
+		
+    break;
+    case GRIPPER:
     
     break;
-    case y:
+    case TURRET:
     
     break;
     case default:
-    
+		// ignore ????
     break;
   }
 }
@@ -76,6 +104,7 @@ void interpretData(piDataType dataType, message) {
 
 // sensor Compass, orientation
 int sensCompass() {
+	// average for last x ????????
   int degree = 359;
   sendData(COMPASS, degree);
   return degree;
@@ -113,6 +142,7 @@ int sensIRRight() {
 int sensSampleTurret() {
   boolean sample = false;
   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);
   }
@@ -158,5 +188,39 @@ void setup() {
 }
 
 void loop() {
-  
+	sensCompass();
+	sensObstacleTurret();
+	sensIRLeft();
+	sensIRRight();
+	sensSampleTurret();
+	sensSampleGripper();
+	sensBeaconTurret();
+	sensLab();
+	
+	if((operationChange + OPERATION_MAX_CHANGE_TIME) > millis() && operationMode != RANDOM) {
+		operationMode = RANDOM;
+	}
+	
+}
+
+// **********************
+// ** HELPER FUNCTIONS **
+// **********************
+
+long microsecondsToInches(long microseconds)
+{
+  // According to Parallax's datasheet for the PING))), there are
+  // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
+  // second).  This gives the distance travelled by the ping, outbound
+  // and return, so we divide by 2 to get the distance of the obstacle.
+  // See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
+  return microseconds / 74 / 2;
+}
+
+long microsecondsToCentimeters(long microseconds)
+{
+  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
+  // The ping travels out and back, so to find the distance of the
+  // object we take half of the distance travelled.
+  return microseconds / 29 / 2;
 }