Revision 1:162879c2e17c, committed 2015-10-13

Comitter:

benparkes

Date:

Tue Oct 13 14:56:48 2015 +0000

Parent:

0:aca11cc796fd

Commit message:

13/10/15 interupts

Changed in this revision

--- a/main.cpp	Tue Oct 13 12:09:44 2015 +0000
+++ b/main.cpp	Tue Oct 13 14:56:48 2015 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 
 #define wheelc 0.1759292
+#define pi 3.141592654
 //Status LED
 DigitalOut led(LED1);
 
@@ -13,9 +14,9 @@
 DigitalOut DIRB(PB_10);
 
 //Hall-Effect Sensor Inputs
-DigitalIn HEA1(PB_2);
+InterruptIn HEA1(PB_2);
 DigitalIn HEA2(PB_1);
-DigitalIn HEB1(PB_15);
+InterruptIn HEB1(PB_15);
 DigitalIn HEB2(PB_14);
 
 //On board switch
@@ -39,72 +40,106 @@
 float dutyA = 1.0f; //100%
 float dutyB = 1.0f; //100%
 
-int forward(float distance, int speed)
+//distance measurement
+float distanceA ;
+float distanceB ;
+float speedA ;
+float speedB ;
+float wheel_spacing = 0.128;
+
+int forward(float distance, float duty)
+{
+    int i;
+        // Set motor direction forward
+        DIRA = FORWARD;
+        DIRB = FORWARD;
+        // Set motor speed to input speed
+    PWMA.write(duty);          //Set duty cycle (%)
+    PWMB.write(duty);          //Set duty cycle (%)
+        //reset distance
+        distanceA = 0;
+        distanceB = 0;
+        timer.reset();
+        timer.start();
+        //wait for run to complete
+        while (((distanceA+distanceB)/2) < distance)
+        {
+            i++;
+        }
+        return 1;
+}
+
+int turn(int degrees,float duty, float distance_outside_wheel_turn_point)
 {
-    //Set motor period to 100Hz
-    dutyA = 5;
-    dutyB = 5;
-    PWMA.period_ms(speed);
-    PWMB.period_ms(speed);
+        //Set initial motor direction
+    DIRA = FORWARD;
+    DIRB = FORWARD;
+    
+        float C1 = (pi * 2 * distance_outside_wheel_turn_point)*(degrees/360);
+        float C2 = (pi * 2 * (distance_outside_wheel_turn_point-wheel_spacing))*(degrees/360);
+    //Set motor speed
+    PWMA.write(duty);          //Set duty cycle (%)
+    PWMB.write(duty*(C2/C1));          //Set duty cycle (%)
+        //reset distance
+        distanceA = 0;
+        distanceB = 0;
+        timer.reset();
+        timer.start();
+        //wait for turn to complete
+        while (distanceA < C1)
+        {
+        }
+                    return 1;
+}
 
-    //Set initial motor speed to stop
-    PWMA.write(dutyA);          //Set duty cycle (%)
-    PWMB.write(dutyB);          //Set duty cycle (%)
-
+void set_distanceA()
+{
+        distanceA += (wheelc/6);
+    terminal.printf("A =%f\n\r",distanceA);
+}
+    
+void set_distanceB()
+{
+        distanceB += (wheelc/6);
+    terminal.printf("B =%f\n\r",distanceB);
 
 }
 
-int turn(int degrees, bool direction)
-{
-    //Set motor period to 100Hz
-    dutyA = 10;
-    dutyB = 10;
-    PWMA.period_ms(200);
-    PWMB.period_ms(0);
 
-    //Set initial motor speed to stop
-    PWMA.write(dutyA);          //Set duty cycle (%)
-    PWMB.write(dutyB);          //Set duty cycle (%)
-    wait(0.676);
-    dutyA = 0;
-    dutyB = 0;
-    PWMA.write(dutyA);          //Set duty cycle (%)
-    PWMB.write(dutyB);          //Set duty cycle (%)
-}
 int main()
 {
     //Configure the terminal to high speed
     terminal.baud(115200);
 
-    //Set initial motor direction
-    DIRA = FORWARD;
-    DIRB = FORWARD;
-
+        HEA1.rise(set_distanceA);
+        HEB1.rise(set_distanceB);
     //Set initial motor speed to stop
     PWMA.write(0.0f);           //0% duty cycle
     PWMB.write(0.0f);           //0% duty cycle
+        PWMA.period_ms(10);
+    PWMB.period_ms(10);
     while(1) {
+            //wait for switch press
         while (SW1 == PRESSED) {
             wait(0.01);
             while(SW1 == RELEASED) {
 
-                for (int i = 0; i<2; i++) {
-                    forward(1,1);
-                    wait(1.5);
-                    turn(90,1);
-                    forward(1,1);
-                    wait(0.75);
-                    turn(90,1);
-
-                }
-                sleep();
+                                //navigate course
+                //for (int i = 0; i<2; i++) {
+                    forward(10,50);
+                            /*              terminal.printf("turn\n\r");
+                    turn(90,50,0.20);
+                                            terminal.printf("forward\n\r");
+                    forward(1,50);
+                                            terminal.printf("turn\n\r");
+                    turn(90,50,0.20);*/
+                                //}
+                                PWMA.write(0.0f);           //0% duty cycle
+                                PWMB.write(0.0f); 
+                                wait(10);
+                                //victory spin
+               // turn(360,50,0.13);
             }
         }
     }
-    //Set initial motor speed to stop
-    PWMA.write(0);          //Set duty cycle (%)
-    PWMB.write(0);          //Set duty cycle (%)
-
-
-
-}
+    }