Diff: main.cpp

Revision:

11:f5dcf8811a4e

Parent:

10:37e7c46837dc

Child:

12:c9d0b1ff36f2

diff -r 37e7c46837dc -r f5dcf8811a4e main.cpp
--- a/main.cpp	Mon Mar 10 20:43:52 2014 +0000
+++ b/main.cpp	Wed Mar 12 10:35:47 2014 +0000
@@ -57,15 +57,24 @@
 float servoAdj[2] = {0.03, 0.03};
 bool mCommandCompleted[2] = {true, true};
 
+const float SERVO_SPEED_ADJ = 0.001;
+
+float servoSpeed = 1 * SERVO_SPEED_ADJ;
+
 int main()
 {
+    printf("MAIN START\n\r");
     sensor_init[0] = new PS2MS_INIT(p18, p17);
+    printf("SENSOR_INIT 0 DONE\n\r");
     sensor_init[1] = new PS2MS_INIT(p23, p22);
+    printf("SENSOR_INIT 1 DONE\n\r");
     sensor_init[2] = new PS2MS_INIT(p26, p25);
+    printf("SENSOR_INIT 2 DONE\n\r");
+    printf("SENSOR_INIT DONE\n\r");
     sensor[0] = new PS2MS(p18, p17);
     sensor[1] = new PS2MS(p23, p22);
     sensor[2] = new PS2MS(p26, p25);
-
+    printf("SENSOR DONE\n\r");
     //TODO: receive all pending packets here
 
     float range = 0.00085;
@@ -81,13 +90,9 @@
     servoPitch.calibrate(range, 45.0);
     servoYaw = servoPos[SERVO_YAW];
     servoPitch = servoPos[SERVO_PITCH];
-    printf("position = %.3f, range = +/-%0.5f\n", position1, range);
-
-    printf("IMHERE START\n");
 
     sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
     int dir;
-    float servoSpeed = 0.002;
 
     bool awaitingPackets = false;
 
@@ -208,11 +213,18 @@
              if (rotateAngleNum > 90 || rotateAngleNum < -90) {
                  return;
              }
-             rotateAngleNum = 0.5 + rotateAngleNum / 90.;
+             rotateAngleNum = 0.5 + rotateAngleNum / 180.;
              servoPos[servoNum] = rotateAngleNum + servoAdj[servoNum];
              mCommandCompleted[servoNum] = false;
         }
     }
+    else if (c == 's') {
+        char buffer[100];
+        pc.gets(buffer, 100);
+        int speedStep = atoi(buffer);
+        servoSpeed = speedStep * SERVO_SPEED_ADJ;
+        printf("Servo speed set to %d\n\r", speedStep);
+    }
 }
 
 int sendCommand(int ind, char command)
@@ -235,7 +247,7 @@
         sendResend(ind);
         successful = processACKReply(ind);
         if (!successful) {
-            printf("DUNNO WHAT TO DO ACK Reply %d\n\r", ind);
+            if (DEBUG) printf("DUNNO WHAT TO DO ACK Reply %d\n\r", ind);
             //wait(1);
             sendCommand(ind, '\xEB');
             return getPacket(ind);