Diff: main.cpp

Revision:

1:f4e3365155e1

Parent:

0:e79700919e2e

diff -r e79700919e2e -r f4e3365155e1 main.cpp
--- a/main.cpp	Fri Oct 16 12:39:12 2015 +0000
+++ b/main.cpp	Sun Oct 18 18:23:58 2015 +0000
@@ -1,6 +1,8 @@
 #include "mbed.h"
 
+
 #define wheelc 0.1759292
+#define wheel_spacing 0.128 //add this in
 #define pi 3.141592654
 #define degreel 0.021988888
 //Status LED
@@ -16,9 +18,9 @@
 
 //Hall-Effect Sensor Inputs
 InterruptIn HEA1(PB_2);
-DigitalIn HEA2(PB_1);
+InterruptIn HEA2(PB_1);
 InterruptIn HEB1(PB_15);
-DigitalIn HEB2(PB_14);
+InterruptIn HEB2(PB_14);
 
 //On board switch
 DigitalIn SW1(USER_BUTTON);
@@ -31,183 +33,271 @@
 
 //Enumerated types
 enum DIRECTION   {FORWARD=0, REVERSE};
-enum PULSE           {NOPULSE=0, PULSE};
+enum PULSE       {NOPULSE=0, PULSE};
 enum SWITCHSTATE {PRESSED=0, RELEASED};
 
 //Debug GPIO
 DigitalOut probe(D10);
 
 //Duty cycles
-float dutyA = 1.000f; //100%
-float dutyB = 1.000f; //100%
+float dutyA = 0.000f; //100%
+float dutyB = 0.000f; //100%
 
 //distance measurement
 float distanceA ;
 float distanceB ;
 float speedA ;
 float speedB ;
-float pretimerA;
-float afttimerA;
-float pretimerB;
-float afttimerB;
-float wheel_spacing = 0.128;
+
+//for interrupt speed and distance calcs
+int tA1[2];
+int tA2[2];
+int tB1[2];
+int tB2[2];
+int pulse_orderA = 1;
+int pulse_orderB = 1;
+
+//interupt handler
+int HEPulse(int let,int num, int order) //tell us which hall effect HEA1= hall effect, (let, A=1, B=2), (num, 1/2), order (index possition to be filled alternates)
+{
+    float fA1;
+    float fA2;
+    float fB1;
+    float fB2;
+    int time;
+    time = timer.read_us(); //use to check for interrupt debounce
+
+    if((tA1[0]-time<5)||(tA2[0]-time<5)||(tA1[1]-time<5)||(tA2[1]-time<5)) {
+        return 1;
+    } // if less than 5us from last interupt assume debouce and ignore
+    //find right thing to do for current hall effect
+    switch(order) { //check whether to fill index possition 0/1
+        case(1) :
+            switch(let) { //check hall effect A/B
+                case(1):
+
+                    switch(num) { //check hall effect 1/2
+                        case (1):
+                            tA1[0] = timer.read_us();
+                            distanceA += (wheelc/6); //increment distance
+                            pulse_orderA = 2; //toggle index to be filled
+                            break;
+
+                        case(2) :
+                            tA2[0] = timer.read_us();
+                            pulse_orderA = 2;
+                            break;
+                    }
+                    break;
+
+
+                case(2): //check hall effect A/B
+
+                    switch(num) {
+                        case (1):
+                            tB1[0] = timer.read_us();
+                            distanceB += (wheelc/6);
+                            pulse_orderB = 2;
+                            break;
 
-//Completed Loop
-int loop=0;
+                        case(2) :
+                            tB2[0] = timer.read_us();
+                            pulse_orderB = 2;
+                            break;
+                    }
+                    break;
+
+            }
+        case(2) :
+            switch(let) {
+                case(1):
+
+                    switch(num) {
+                        case (1):
+                            tA1[1] = timer.read_us();
+                            distanceA += (wheelc/6);
+                            fA1 = 1.0f/(( fabsf(tA1[1]-tA1[0]) ) * (float)3.0E-6);
+                            pulse_orderA = 1;
+                            break;
+
+                        case(2) :
+                            tA2[1] = timer.read_us();
+                            fA2 = 1.0f/(( fabsf(tA2[1]-tA2[0]) ) * (float)3.0E-6);
+                            pulse_orderA = 1;
+                            break;
+                    }
+                    break;
+
 
-int turn();
-void ResetDistanceTimeSpeed()
+                case(2):
+
+                    switch(num) {
+                        case (1):
+                            tB1[1] = timer.read_us();
+                            distanceB += (wheelc/6);
+                            fB1 = 1.0f/(( fabsf(tB1[1]-tB1[0]) ) * (float)3.0E-6);
+                            pulse_orderB = 1;
+                            break;
+
+                        case(2) :
+                            tB2[1] = timer.read_us();
+                            fB2 = 1.0f/((fabsf( tB2[1]-tB2[0]) ) * (float)3.0E-6);
+                            pulse_orderB = 1;
+                            break;
+                    }
+                    break;
+
+            }
+
+            float fA = (fA1 + fA2)*0.5f; //calculate average frequency of hall effect A
+            float fB = (fB1 + fB2)*0.5f; //calculate average frequency of hall effect B
+            speedA = fA/20.8f; //output speeds of motors by frequency/gear box ratio
+            speedB = fB/20.8f;
+    }
+    return 1;
+}
+
+void ResetDistanceTimeSpeed() //reset vaviables for new straight or turn
 {
     distanceA=0;
     distanceB=0;
     speedA=0;
     speedB=0;
-    pretimerA=0;
-    pretimerB=0;
-    afttimerA=0;
-    afttimerB=0;
     timer.reset();
     timer.start();
 }
 
-void stopmotors()
+void stopmotors() //stop motors dead
 {
     PWMA.write(0.0f);       //0% duty cycle
     PWMB.write(0.0f);       //0% duty cycle
 }
 
-int forward(float distance, float speed)
+void HuntSpeeds(float target_speedA, float target_speedB) //alter speeds to desired speed by hunting for corresponding duty (input desired speeds)
 {
-    //add forward to input with switch for scalability
+
+    float deltaA = target_speedA -speedA; //find difference between actual speed and desired speed
+    dutyA = dutyA + deltaA*0.1f; //modify duty dependant upon size of error
+    PWMA.write(dutyA); //write new duty to motor
+
+    float deltaB = target_speedB -speedB; //repeat for B motor
+    dutyB = dutyB + deltaB*0.1f;
+    PWMB.write(dutyB);
+}
+
+
+int forward(float distance, float speed) //forward motion, input distance and speed (rev/s)
+{
     // Set motor direction forward
     DIRA = FORWARD;
     DIRB = FORWARD;
 
-        //reset distance
+    //reset distance
     ResetDistanceTimeSpeed();
-    
-    // Set motor speed to input speed
-    PWMA.write(0.1);          //Set duty cycle (%)
-    PWMB.write(0.1);          //Set duty cycle (%)
+
+    // Set initial motor to be altered by speed code
+    PWMA.write(0.1f*speed);          //Set duty cycle (%)
+    PWMB.write(0.1f*speed);          //Set duty cycle (%)
 
 
-    //wait for run to complete
+    //wait for run to complete byy checking if distance has been covered yet
     while (((distanceA+distanceB)/2) < distance) {
-        
-            if (speedA<speed){
-                                   
-                    dutyA += (float)0.0051;
-          PWMA.write(dutyA);
-            }
-            if( speedA>speed){
-                dutyA -= (float)0.0051;
-        PWMA.write(dutyA);
-            }
-            
-            if (speedB<speed){
-                                   
-                    dutyB += (float)0.0051;
-          PWMB.write(dutyB);
-            }
-            if( speedB>speed){
-                dutyB -= (float)0.0051;
-        PWMB.write(dutyB);
-            }
-            
-    }
-        return 1;
-    }
-
-int turn(float degrees, float duty, int direction) // (Degrees of Turn, Speed, (Anti/Clock)wise)
- 
-{
-    // Calculate Turn Distance
-    float distance=0;
-    distance=((float)degreel*degrees);
-    //Set Initial Motor Direction
-    DIRA = FORWARD;
-    DIRB = FORWARD;
- 
-    // Test for Loop Completion to Enter Victory Spin
-    if(direction==REVERSE) {
-        DIRA = REVERSE;
-        DIRB = REVERSE;
-    }
- 
-    // Set Motor Speed for Outside Wheel
-    PWMA.write(duty);          //Set duty cycle (%)
-    PWMB.write(0.0f);          //Set duty cycle (%)
- 
-    // Reset Distance Travelled
-        ResetDistanceTimeSpeed();
-    // Wait for Turn to Complete
-    while (distanceA < distance) {
+        //maintain desired speed
+        HuntSpeeds(speed, speed);
     }
     return 1;
 }
 
-    void set_distanceA() {
-        float time = 0;
+int turn(float degrees, float speed, int direction,float turn_radius) // (Degrees of Turn, Speed, (Anti/Clock)wise, turn radius (m))
+
+{
+    // Calculate Turn Distance
+    float C1 = (pi * 2 * (float)turn_radius)*(degrees/360); // outside arc (piD)/fraction turn
+    float C2 = (pi * 2 * ((float)turn_radius-(float)wheel_spacing))*(degrees/360); //inside arc (piD)/fraction turn
+    //Set Initial Motor Direction
+    DIRA = FORWARD;
+    DIRB = FORWARD;
 
-        afttimerA = timer.read();                      //set latest timer to equal timer
-        distanceA += (wheelc/6);                //set distance travelled for this instruction i.e forward/turn etc
-        time = afttimerA - pretimerA;             //work out time taken for last 6th of a rotation
-        speedA = (time)/6;              //distance/time = average speed for last 6th rotation
-        pretimerA = afttimerA;                  //update pretimer for next calculation of time
-                terminal.printf("speedA %f\n\r", speedA);
+    // Test for direction to Enter Victory Spin
+    if(direction==REVERSE) {
+        DIRA = REVERSE;
+        DIRB = FORWARD;//victory spin opposite wheels
+    }
+    // Reset Distance Travelled
+    ResetDistanceTimeSpeed();
+
+    // Wait for Turn to Complete by checking whether outer wheel completed spin yet
+    while (distanceA < C1) {
+        if(direction==FORWARD) {
+            HuntSpeeds(speed, speed*(float(C2/C1))); //maitain speeds proportional to difference in turn length for each wheel
+        }
+
+        if(direction==REVERSE) {
+            HuntSpeeds(speed,speed); //set speeds same for reverse spin
+        }
     }
-
-    void set_distanceB() {
-        float time = 0;
+    return 1;
+}
+//interupt handlers
+void HEA1I(void)
+{
+    HEPulse(1,1,pulse_orderA);
+}
+void HEA2I(void)
+{
+    HEPulse(1,2,pulse_orderA);
+}
+void HEB1I(void)
+{
+    HEPulse(2,1,pulse_orderB);
+}
+ void HEB2I(void)
+{
+    HEPulse(2,2,pulse_orderB);
+}
 
-        afttimerB = timer.read();
-        distanceB += (wheelc/6);
-        time = afttimerB - pretimerB;
-        speedB = time/6;
-        pretimerB = afttimerB;
-            terminal.printf("speedB %f\n\r", speedB);
+int main()
+{
+    //Configure the terminal to high speed
+    terminal.baud(115200);
+    // initiate interupts to call interupt code
+    HEA1.rise(&HEA1I);
+    HEA2.rise(&HEA2I);
+    HEB1.rise(&HEB1I);
+    HEB2.rise(&HEB2I);
+    //Set initial motor speed to stop
+    stopmotors();
+    //set motor frequency
+    PWMA.period_ms(10);
+    PWMB.period_ms(10);
+    ResetDistanceTimeSpeed();
+    while(1) {
+    //wait for switch press
+    while (SW1 == PRESSED) {
+            wait(0.01);
+            while(SW1 == RELEASED) {
+
+                //navigate course
+                for (int i = 0; i<2; i++) {
+                    //forward 1m
+                    forward(12,1); //distance, speed in rotations/s
+                    //turn 90
+                    turn(90,0.5,0,0.2);//degrees, speed (rev/s), direction (0 clockwise),radius of turn)
+                    //forward 1/2m
+                    forward(7,1);//distance, speed in rotations/s
+                    //turn 90
+                    turn(90,0.5,0,0.2);//degrees, speed (rev/s), direction (0 clockwise),radius of turn)
+                }
+                //repeat twice to end up back in starting possition
+
+                //victory spin
+                turn(365,0.5,1,0.001);//degrees, speed (rev/s), direction (0 clockwise),radius of turn)
+
+                stopmotors();
+                break;
+            }
+        }
     }
 
-
-    int main() {
-        //Configure the terminal to high speed
-        terminal.baud(115200);
-
-        HEA1.rise(set_distanceA);
-        HEB1.rise(set_distanceB);
-        //Set initial motor speed to stop
-        stopmotors();
-        PWMA.period_ms(10);
-        PWMB.period_ms(10);
-        while(1) {
-            //wait for switch press
-            while (SW1 == PRESSED) {
-                wait(0.01);
-                while(SW1 == RELEASED) {
+}
 
-                    //navigate course
-                    for (int i = 0; i<2; i++) {
-                        forward(12,0.01);
-                        //terminal.printf("turn\n\r");
-                        turn(100,1,0);
-                        //terminal.printf("forward\n\r");
-                        forward(7,0.01);
-                        //terminal.printf("turn\n\r");
-                        turn(100,1,0);
-                    }
-
-                    stopmotors();
-
-                    wait(0.5);
-
-                    //victory spin
-                    turn(365,1,1);
-
-                    stopmotors();
-                                        break;
-                }
-            }
-        }
-                
-    }
-