Diff: main.cpp

Revision:

2:054c41590f3f

Parent:

1:c4af0bbe20b9

--- a/main.cpp	Wed Mar 11 22:41:53 2015 +0000
+++ b/main.cpp	Thu Mar 26 15:52:33 2015 +0000
@@ -13,28 +13,106 @@
 Serial blue(PTC4,PTC3);
 DigitalOut led(LED1);
 
+
+void manual_mode()
+{
+    while( blue.readable() )
+        int direction = 0x11;
+    unsigned char rem_dir; //remote_direction
+
+    blue.printf("Thanks, calculating it now, you entered %c\n",rem_dir);
+    if(   ( rem_dir == 'f')   ) {
+        direction = 0x11;
+        blue.printf("You wanted to go straight\n");
+        switch (rem_dir) {
+
+            case('f'):
+                direction = 0x11;
+                blue.printf("You wanted to go straight\n");
+                break;
+            case( 'b' )    :
+                direction = 0x00;
+                blue.printf("You wanted to go backwards\n");
+                break;
+            case( 'l' )  :
+
+                direction = 0x01;
+                blue.printf("You wanted to spin anti-clockwise\n");
+
+                break;
+            case ('r') :
+
+                direction = 0x10;
+                blue.printf("You wanted to spin clockwise\n");
+                break;
+            case ('s') :
+                motor_r.write(0);
+                motor_l.write(0);
+                blue.printf("Stopping now.\n");
+                break;
+        }
+        if (   (rem_dir == 'b' )     ) {
+
+            direction = 0x00;
+            blue.printf("You wanted to go backwards\n");
+        }
+        if (   (rem_dir == 'l' )   ) {
+
+            direction = 0x01;
+            blue.printf("You wanted to spin anti-clockwise\n");
+
+        }
+        if (   (rem_dir == 'r' )   )  {
+
+            direction = 0x10;
+            blue.printf("You wanted to spin clockwise\n");
+        }
+        if (rem_dir =='s') {
+
+            motor_r.write(0);
+            motor_l.write(0);
+            blue.printf("STAAAAHP\n");
+        } else {
+            wait(0.1);
+            wait(0.1);
+            blue.printf("What duty left?\n");
+            while ( blue.readable()==0 ) {}               // loops until there is an input from the user
+            char l_speed = blue.getc();
+            int l_result = l_speed - '0';
+            int r_result = r_speed - '0';
+            set_direction(direction, (float)l_result/10, (float)r_result/10);
+        }
+
+    }
+
+}
+
+}
+
+}
+
+
+
 // which target are we at?
-int TargetChoice()
+void TargetChoice()
 {
 
     blue.printf("Which target am I at?");           // ask user question
     char TargetIn = blue.getc();                    // assign user input to TargetIn
-    int command=0, choice = 0;
+    int command=0;
 
     while(command==0) {                             // loop until a command has been given
         switch (TargetIn) {                         // input is either 1 or 2 in DECIMAL
             case 0x31:                              // if target 1
-                choice=1;                           //
+                closetarget();                           //
                 command = 1;                        // exit while
                 break;
             case 0x32:                              // if target 2
-                choice=2;                            // return 2 to ball firing or call ball fire 2
+                fartarget();                            // return 2 to ball firing or call ball fire 2
                 command = 1;                        // exit while
                 break;
         }
-
     }
-    return choice;                                  // return which target to fire at
 }
 
 // did we hit the target
@@ -114,73 +192,6 @@
 //LED to test
 DigitalOut myled(LED_BLUE);
 
-void set_direction( int direction, float speed, float angle)
-{
-    /* *******************Example Use*****************
-                 set_direction("11", 0.5, 0);  //This would set the robot to go forward, at half speed, straight
-
-                 set_directin( "11", 0.8, 0.2); //this would set robot to go forward, at 100% to the right and 60% to the left (turn right)
-
-                 set_direction("00", 0.3, 0.7); //robot back, 100% rightmotor; 0%left (technically -40%, but 0% is min)
-
-                 set_direction("11", 0.5, 0.5); // robot pivots forward, to the right, full speed (100% duty right ; 0% duty left)
-
-                 set_direction("11", 0.5, -0.5); //robot pivots forward, to the left,  full speed  (0% duty right ; 100% duty left)
-
-                 set_direction("11", 0.3, 0.5); //robot pivots forard, to the right , at 80% speed (80% right; 0% left)
-
-                 set_direction("11", -0.3, 0.5); //robot pivotrs foward, to the right, AT 20% SPEED (20%, 0%)
-
-                 set_direction("10", 0.5, 0);   //robot spins to teh right, at half speed. Left forward 50% ; Right forward 50%
-
-                 set_direction("10", 0.5, 0.3); //robot spins to the right, 80% right forward ; 20% left backward
-
-    */
-
-    float r_result = (speed + angle); //where angle can be postive and negative and will TURN the robot
-    float l_result = (speed - angle); //if turning left, you'd make the angle postive (right motor goes faster left motor goes slwoer)
-    switch( direction ) {
-        case 0x11: { //forward
-            motor_r.write(  r_result  < 0 ? 0    :     r_result > 1 ? 1    :       r_result);
-            //Look up ternary statements -- {condition ? value_if_true : value_if_false} This makes 0<result<1
-            motor_l.write(  l_result < 0 ? 0     :     l_result > 1 ? 1    :       l_result);
-
-            motor_rf=1;
-            motor_rb=0;
-            motor_lf=1;
-            motor_lb=0;
-        }
-        case 0x00: { //backward
-            motor_r.write( r_result  < 0 ? 0    :     r_result > 1 ? 1    :       r_result);
-            motor_l.write( l_result  < 0 ? 0    :     l_result > 1 ? 1    :       l_result);
-
-            motor_rf=0;
-            motor_rb=1;
-            motor_lf=0;
-            motor_lb=1;
-        }
-        case 0x01: {  //spin left  --     Right forward, left backward
-            motor_r.write( r_result  < 0 ? 0    :     r_result > 1 ? 1    :       r_result);
-            motor_l.write( l_result  < 0 ? 0    :     l_result > 1 ? 1    :       l_result);
-
-            motor_rf=1;
-            motor_rb=0;
-            motor_lf=0;
-            motor_lb=1;
-        }
-        case 0x10: {    //spin right
-            motor_r.write( r_result  < 0 ? 0    :     r_result > 1 ? 1    :       r_result);
-            motor_l.write( l_result  < 0 ? 0    :     l_result > 1 ? 1    :       l_result);
-
-            motor_rf=0;
-            motor_rb=1;
-            motor_lf=1;
-            motor_lb=0;
-        }
-    }
-}
-
-
 void SpinFunct()
 {
     //Set PWM frequency to 1000Hz
@@ -198,122 +209,7 @@
 
 }
 
-void JiveFunct()
-{
-
-    //Set PWM frequency to 1000Hz
-    motor_l.period( 1.0f / (float) PWM_FREQ);
-    motor_r.period( 1.0f / (float) PWM_FREQ);
-    //Initialise direction to nothing.
-    motor_rf=0;
-    motor_rb=0;
-    motor_lf=0;
-    motor_lb=0;
-
-    //forward + right spin
-    wait(2);
-    myled = !myled;
-    set_direction(0x11, 0.3, 0.5); //robot pivots forard, to the right , at 80% speed (80% right; 0% left)
-
-    // back spin
-    wait(2);
-    myled = !myled;
-    set_direction(0x00, 0.3, 0.7); //robot back, 100% rightmotor; 0%left (technically -40%, but 0% is min)
-
-    //
-    wait(2);
-    myled = !myled;
-    set_direction(0x01, 0.3, 0.4);   //robot spins to teh left, at half speed. Left forward 70% ; Right 0% (technically -30%, but 0% is min)
-
-    wait(2);
-    myled = !myled;
-    set_direction(0x10, 0.5, 0);   //robot spins to teh right, at half speed. Left forward 50% ; Right forward 50%
-
-
-    /*
-     wait(2);
-     led = !led;
-     set_direction(0x11, 0.5, 0);  //This would set the robot to go forward, at half speed, straight
-
-     wait(2);
-     led = !led;
-     set_direction(0x11, 0.8, 0.2); //this would set robot to go forward, at 100% to the right and 60% to the left (turn right)
-
-     wait(2);
-     led = !led;
-     set_direction(0x11, 0.5, 0.5); // robot pivots forward, to the right, full speed (100% duty right ; 0% duty left)
-
-     wait(2);
-     led = !led;
-     set_direction(0x11, 0.5, -0.5); //robot pivots forward, to the left,  full speed  (0% duty right ; 100% duty left)
-
-
-
-     wait(2);
-     led = !led;
-     set_direction(0x11, -0.3, 0.5); //robot pivotrs foward, to the right, AT 20% SPEED (20%, 0%)
-
-     wait(2);
-     led = !led;
-     set_direction(0x10, 0.5, 0);   //robot spins to teh right, at half speed. Left forward 50% ; Right forward 50%
-
-     wait(2);
-     led = !led;
-     set_direction(0x10, 0.5, 0.3); //robot spins to the right, 80% right forward ; 20% left backward
-     */
-
-}
-
-
-// will just make a question at the end to say quit? yes or no. it's possible to make a quitter function and test the input for quit
-// at every point but it's too annoying to get it to work... so fuk dat
-
-void RemoteControl()
-{
-    char dir[2]; /*speed=0, angle=0, time=0, */
-    int quit=0;
-
-    while(quit==0) {
-        wait(1);
-        //dir
-        blue.printf("Direction?\n");
-        while ( blue.readable()==0 ) {}               // loops until there is an input from the user
-
-        for(int i=0; i<2; i++) {
-            dir[i] = blue.getc();    //Store incoming response into array
-        }
-        blue.printf("%c dir\n", dir);
-
-    }
-}
-//for(int i=0; i<4; i++) { quitarray[i]= blue.getc();  blue.printf("get %c", quitarray[i]); } //Store incoming response into array
-//quit=Quitter(quitarray);
-
-/*
-//speed
-blue.printf("Speed?");
-while ( blue.readable() ) {}               // loops until there is an input from the user
-    speed=blue.getc();
-    quit=Quitter();
-
-
-//angle
-blue.printf("Angle?");
-while (blue.readable()==0) {}               // loops until there is an input from the user
-    angle=blue.getc();
-    quit=Quitter();
-
-
-//time
-blue.printf("How long?");
-while (blue.readable()==0) {}               // loops until there is an input from the user
-    time=blue.getc();
-    quit=Quitter();
-*/
-//set_direction(dir, speed, angle); //also find a way to do time too
-//wait(time);
-
-
+// cant send 10 in ascii or in hex as they
 /*
 
 send 11, 0.5, 0.2 in one string
@@ -349,11 +245,16 @@
 {
 
     int spin[4] = {0x73, 0x70, 0x69, 0x6e};
-    int jive[4] = {0x6a, 0x69, 0x76, 0x65};
     int ctrl[4] = {0x63, 0x74, 0x72, 0x6c};
     int maze[4] = {0x6d, 0x61, 0x7a, 0x65};
     unsigned char x[4];
 
+
+//Set PWM frequency to 1000Hz
+    motor_rb=0;
+    motor_lf=0;
+    motor_lb=0;
+
     while (1) {
         blue.printf("What will I do?\n");           // ask user question
         while (blue.readable()==0) {}               // loops until there is an input from the user
@@ -362,15 +263,13 @@
             x[i] = blue.getc();     //Store incoming response into array
         }
 
-        int FindSpin=0, FindJive=0, FindCtrl=0, FindMaze=0;
+        int FindSpin=0, FindCtrl=0, FindMaze=0;
 
         for(int s=0; s<4; s++) {
             if (x[s]==spin[s]) {
                 FindSpin++;
             }
-            if (x[s]==jive[s]) {
-                FindJive++;
-            }
+
             if (x[s]==ctrl[s]) {
                 FindCtrl++;
             }
@@ -382,38 +281,15 @@
         if (FindSpin==4) {
             SpinFunct();
         }
-        if (FindJive==4) {
-            JiveFunct();
+
+        if (FindCtrl==4) {
+            manual_mode() ;
         }
-        if (FindCtrl==4) {
-            RemoteControl() ;
-        }
+
         if (FindMaze==4) {
             led=1;
             wait(4);
             led=0;
         }
     }
-}
-
-
-
-
-
-
-
-
-
-
-/*
-int Quitter(char x[4]) {     int quit=0; char quitting[4]={0x71, 0x75, 0x69, 0x74};
-
-    blue.printf ("works");
-    for(int i=0; i<4; i++) {  x[i] = blue.getc();  } //Store incoming response into array
-    for(int s=0; s<4; s++) {
-        if (x[s]==quitting[s]) { quit++; }
-    }
-    if (quit==4) { quit=1;}
-    return quit;
-}
-*/
\ No newline at end of file
+}
\ No newline at end of file