From f6655dbb1377bc84602c0e7b823a10acc3b8c2b0 Mon Sep 17 00:00:00 2001
From: Peter Wu <peter@lekensteyn.nl>
Date: Fri, 12 Jun 2015 12:19:09 +0200
Subject: simplify state change from uninit

---
 Venus_Skeleton/Venus_Skeleton.ino | 33 +++++++++++----------------------
 1 file changed, 11 insertions(+), 22 deletions(-)

diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino
index 7718d8a..c3832c1 100644
--- a/Venus_Skeleton/Venus_Skeleton.ino
+++ b/Venus_Skeleton/Venus_Skeleton.ino
@@ -1,6 +1,5 @@
 // TODO
 //
-// - limit sendData to x per second
 // - errorSequence()
 // - insert into calibration files
 // - Make sure ENUM piDataType is correct and synced
@@ -80,7 +79,7 @@ unsigned long lastCommunication = 0;
 
 // Current operation mode
 opmode_t operationMode = OPMODE_WAIT;
-opmode_t operationModeDefault = OPMODE_INITIALSEQUENCE;
+boolean initialized = false;
 unsigned long operationChange = 0;
 
 // Location values
@@ -191,14 +190,6 @@ void initTurretSequence(int array[], int numOfPositions) {
 // ** PI COMMUNICATION **
 // **********************
 
-// sendData to Raspberry Pi
-void sendData(pi_datatype_t method, int data, int data1, int data2, int data3) {
-	if (1 == 1) {
-		lastCommunication = millis();
-	}
-	
-}
-
 void interpretData(pi_datatype_t dataType, int message) {
 	switch(dataType) {
 		case PI_DATATYPE_MOVETO:
@@ -908,29 +899,27 @@ void loop() {
 	// calculateOrientation();
 	// calculateLocationOffset();
 	
-	if (checkRPiActive()) {
-		operationModeDefault = (operationModeDefault == OPMODE_INITIALSEQUENCE ? OPMODE_INITIALSEQUENCE : OPMODE_WAIT);
-		//sendData(PI_DATATYPE_ALL, 'lala', 'lala');
-		//readData();
-	} else {
-		// STANDALONE ALGORITHM
-		operationModeDefault = (operationModeDefault == OPMODE_INITIALSEQUENCE ? OPMODE_INITIALSEQUENCE : OPMODE_MAPPING);
-	}
-	dataToPiChangedBits = 0; // Clear changed bits
-	
+	// TODO integrate pi communications
 	
 	switch(operationMode) {
 		case OPMODE_WAIT:
 		counterMovement = 0;
 		timerMovementStart = 0;
 		timerMovementStop = 0;
-		opMode(operationModeDefault);
+		if (!initialized) {
+			// whenever the initial sequence to locate the robot has not happened,
+			// keep performing that sequence
+			opMode(OPMODE_INITIALSEQUENCE);
+		} else {
+			// goto standalone algorithm when idle
+			opMode(OPMODE_MAPPING);
+		}
 		turnTurretToNext();
 		break;
 		
 		case OPMODE_INITIALSEQUENCE:
 		initialSequence();
-		operationModeDefault = OPMODE_WAIT;
+		initialized = true;
 		break;
 		
 		case OPMODE_RANDOM:
-- 
cgit v1.2.1