Diff: main.cpp

Revision:

4:31e5dfbe68e8

Parent:

3:d42f51bcf883

--- a/main.cpp	Mon Nov 28 22:41:25 2016 +0000
+++ b/main.cpp	Fri Jan 13 23:16:32 2017 +0000
@@ -6,30 +6,89 @@
 int main() {
     robot.init();
    
+    int count = 0;
+    while(1){wait(1);}
     while(1) {
-        mbed_led1 = 1;
-        /*
-        for(int i=0;i<8;i++){
-          pc.printf("Sensor %d: %d\n",i,sensors.read_adc_value(i));
-          wait(0.05);   
+        switch(count){
+            case 0: 
+            led.set_green_led(0,255);   
+            led.set_green_led(1,255);   
+            led.set_green_led(2,0);   
+            led.set_green_led(3,0);   
+            led.set_green_led(4,0);   
+            led.set_green_led(5,0);   
+            led.set_green_led(6,255);   
+            led.set_green_led(7,255);   
+            led.set_red_led(0,0);   
+            led.set_red_led(1,0);   
+            led.set_red_led(2,0);   
+            led.set_red_led(3,0);   
+            led.set_red_led(4,0);   
+            led.set_red_led(5,0);   
+            led.set_red_led(6,0);   
+            led.set_red_led(7,0);   
+          //  motors.forwards(0.8);
+            break;
+            case 1: 
+            led.set_green_led(0,255);   
+            led.set_green_led(1,255);   
+            led.set_green_led(2,0);   
+            led.set_green_led(3,0);   
+            led.set_green_led(4,0);   
+            led.set_green_led(5,0);   
+            led.set_green_led(6,255);   
+            led.set_green_led(7,255);   
+            led.set_red_led(0,255);   
+            led.set_red_led(1,255);   
+            led.set_red_led(2,0);   
+            led.set_red_led(3,0);   
+            led.set_red_led(4,0);   
+            led.set_red_led(5,0);   
+            led.set_red_led(6,255);   
+            led.set_red_led(7,255);   
+          //  motors.coast();
+            break;
+            case 2: 
+            led.set_green_led(0,0);   
+            led.set_green_led(1,0);   
+            led.set_green_led(2,255);   
+            led.set_green_led(3,255);   
+            led.set_green_led(4,255);   
+            led.set_green_led(5,255);   
+            led.set_green_led(6,0);   
+            led.set_green_led(7,0);   
+            led.set_red_led(0,0);   
+            led.set_red_led(1,0);   
+            led.set_red_led(2,0);   
+            led.set_red_led(3,0);   
+            led.set_red_led(4,0);   
+            led.set_red_led(5,0);   
+            led.set_red_led(6,0);   
+            led.set_red_led(7,0);   
+         //   motors.backwards(0.8);
+            break;
+            case 3: 
+            led.set_green_led(0,0);   
+            led.set_green_led(1,0);   
+            led.set_green_led(2,0);   
+            led.set_green_led(3,0);   
+            led.set_green_led(4,0);   
+            led.set_green_led(5,0);   
+            led.set_green_led(6,0);   
+            led.set_green_led(7,0);   
+            led.set_red_led(0,0);   
+            led.set_red_led(1,0);   
+            led.set_red_led(2,255);   
+            led.set_red_led(3,255);   
+            led.set_red_led(4,255);   
+            led.set_red_led(5,255);   
+            led.set_red_led(6,0);   
+            led.set_red_led(7,0);   
+         //   motors.brake();
+            break;
         }
-        */
-        
-        //robot.debug("Sensor 2: %d\n",sensors.get_adc_value(2));
-        wait(0.1);
-        mbed_led1 = 0;
-        wait(0.1);
-        
-        motors.set_left_motor_speed(0.25);
-        robot.update_status_message();
-        robot.debug("Status message:[");
-        for(int i=0;i<16;i++){
-            robot.debug("%2X ",status_message[i]);
-        }
-        
-        robot.debug("]\n");
-        
-        
-       // pc.printf("V: %3.3f\n",robot.get_battery_voltage());
+        count++;
+        if(count==4) count = 0;
+        wait(0.25);
     }
 }