speed code

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
benparkes
Date:
Sun Oct 18 18:23:58 2015 +0000
Parent:
0:e79700919e2e
Commit message:
new code

Changed in this revision

interupts.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
motions.h Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/interupts.h	Sun Oct 18 18:23:58 2015 +0000
@@ -0,0 +1,103 @@
+
+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<10)||(tA2[0]-time<10)||(tA1[1]-time<10)||(tA2[1]-time<10)) {
+        return 1;
+    } // if less than 10us 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;
+
+                        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;
+
+
+                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;
+    }
+
+
+
+
--- 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;
-                }
-            }
-        }
-                
-    }
-    
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motions.h	Sun Oct 18 18:23:58 2015 +0000
@@ -0,0 +1,82 @@
+
+void ResetDistanceTimeSpeed() //reset vaviables for new straight or turn
+{
+    distanceA=0;
+    distanceB=0;
+    speedA=0;
+    speedB=0;
+    timer.reset();
+    timer.start();
+}
+
+void stopmotors() //stop motors dead
+{
+    PWMA.write(0.0f);       //0% duty cycle
+    PWMB.write(0.0f);       //0% duty cycle
+}
+
+void HuntSpeeds(float target_speedA, float target_speedB) //alter speeds to desired speed by hunting for corresponding duty (input desired speeds)
+{
+
+    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
+    ResetDistanceTimeSpeed();
+
+    // Set initial motor to be altered by speed code
+    PWMA.write(0.1f*speed);          //Set duty cycle (%)
+    PWMB.write(0.1*speed);          //Set duty cycle (%)
+
+
+    //wait for run to complete byy checking if distance has been covered yet
+    while (((distanceA+distanceB)/2) < distance) {
+        //maintain desired speed
+        HuntSpeeds(speed, speed);
+    }
+    return 1;
+}
+
+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 * turn_radius)*(degrees/360); // outside arc (piD)/fraction turn
+    float C2 = (pi * 2 * (turn_radius-wheel_spacing))*(degrees/360); //inside arc (piD)/fraction turn
+    //Set Initial Motor Direction
+    DIRA = FORWARD;
+    DIRB = FORWARD;
+
+    // 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
+        }
+    }
+    return 1;
+}
\ No newline at end of file