![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
speed code
Dependencies: mbed
Revision 1:f4e3365155e1, committed 2015-10-18
- Comitter:
- benparkes
- Date:
- Sun Oct 18 18:23:58 2015 +0000
- Parent:
- 0:e79700919e2e
- Commit message:
- new code
Changed in this revision
diff -r e79700919e2e -r f4e3365155e1 interupts.h --- /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; + } + + + +
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; - } - } - } - - } -
diff -r e79700919e2e -r f4e3365155e1 motions.h --- /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