Diff: minissc.cpp

Revision:

3:5c664d91ba63

Parent:

2:e5458113c26b

--- a/minissc.cpp	Sun Feb 26 02:03:17 2012 +0000
+++ b/minissc.cpp	Mon Jul 08 04:02:04 2013 +0000
@@ -29,8 +29,8 @@
 }
 
 void MiniSSC2::send(int i_motor) {
-    // format: {sync byte, motor id, motor power, newline}
-    // example: {SSC_SYNC_BYTE, 2, 24, '\n'} sets motor 2 to power level 24
+    // format: {sync byte, motor id, motor power}
+    // example: {SSC_SYNC_BYTE, 2, 24} sets motor 2 to power level 24
     p_device->putc(SSC_SYNC_BYTE);
     p_device->putc((unsigned char)i_motor);
     p_device->putc(motors[i_motor]);
@@ -55,25 +55,105 @@
     ssc.send(); // is there a more efficient way to do this?
 }
 
+void led1_on_cb()
+{
+    led1 = 1;
+}
+
+void led1_off_cb()
+{
+    led1 = 0;
+}
+
 /* MAIN FUNCTION */
+// this code is very bad. please do not use it for anything serious.
 int main() {
     led1 = 1;
     wait_ms(500);
     //ssc_to.attach(&ssc_send_cb, 1); // run ssc_send_cb() at 10 Hz (Ticker) (increase if possible)
     PC.baud(115200);
+    PC.printf("\n\r\n\rHello.\n\r");
     
-    PC.printf("\n\r\n\rHello.");
+    led1_on.attach(&led1_on_cb, 1.0f);
+    wait_ms(50);
+    led1_off.attach(&led1_off_cb, 1.0f);
     
-    while (true) {
-        for (int i = 0; i < 4; i++) {
-            unsigned char in = 127;
-            PC.printf("> ");
-            PC.scanf("%d", &in);
-            ssc.set(i, in);
-            PC.printf("\n\rSaved motor %d, power %d\n\r", i, (int)in);
+        ssc.set(1);
+    while (true)
+    {
+        led2 = led3 = led4 = 0;
+        led2 = 1;
+        ssc.send();
+        /*PC.printf("Motors: l = left, r = right, f = forward, b = back.\n\r");
+        PC.printf("Which motor? ");
+        int in = PC.getc();
+        PC.printf("%c\n\r", in);
+        char motorID, motorData;
+        PC.printf("Setting motor ");
+        switch (in)
+        {
+            case 'r':
+                motorID = 0;
+                PC.printf("left.\n\r");
+                break;
+                
+            case 'l':
+                motorID = 1;
+                PC.printf("right.\n\r");
+                break;
+            
+            case 'f':
+                motorID = 2;
+                PC.printf("front.\n\r");
+                break;
+            
+            case 'b':
+                motorID = 3;
+                PC.printf("back.\n\r");
+                break;
+                
+            case 'q':
+                ssc.set(127);
+                ssc.send();
+                break;
+                
+            default:
+                PC.printf("Invalid motor. Try again.\n\r");
+                continue;
+                break;
         }
-        ssc.send();
-        PC.printf("Sent all motors\n\r\n\r");
+        led3 = 1;
+        PC.printf("Power (-100 to 100): ");
+        std::string powerRaw;
+        tryAgain:
+        powerRaw.clear();
+        while (true)
+        {
+            in = PC.getc();
+            PC.putc(in);
+            led4 = 1;
+            if (in == '\r')
+            {
+                PC.putc('\n');
+                break;
+            }
+            powerRaw.push_back(in);
+        }
+        
+        if (sscanf(powerRaw.c_str(), "%d", &in) < 1 || in > 100 || in < -100)
+        {
+            printf("Invalid power. Try again: ");
+            goto tryAgain;
+        }
+        
+        in *= 1.27f;
+        in += 127;
+        
+        //PC.printf("Set motor %d to %d\n\r\n\r", motorID, in);
+        
+        ssc.set(motorID, in);
+        ssc.send(motorID);
+        
+        PC.printf("\n\r--------------------------------\n\r");*/
     }
 }
-// Don't delete this comment