![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
with speed calc
Dependencies: mbed
Revision 0:7118ab174b9c, committed 2015-10-15
- Comitter:
- benparkes
- Date:
- Thu Oct 15 12:13:00 2015 +0000
- Commit message:
- speeds 15/10/15
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
mbed.bld | Show annotated file Show diff for this revision Revisions of this file |
diff -r 000000000000 -r 7118ab174b9c main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Oct 15 12:13:00 2015 +0000 @@ -0,0 +1,194 @@ +#include "mbed.h" + +#define wheelc 0.1759292 +#define pi 3.141592654 +#define arcl 1.979203372 +//Status LED +DigitalOut led(LED1); + +//Motor PWM (speed) +PwmOut PWMA(PA_8); +PwmOut PWMB(PB_4); + +//Motor Direction +DigitalOut DIRA(PA_9); +DigitalOut DIRB(PB_10); + +//Hall-Effect Sensor Inputs +InterruptIn HEA1(PB_2); +DigitalIn HEA2(PB_1); +InterruptIn HEB1(PB_15); +DigitalIn HEB2(PB_14); + +//On board switch +DigitalIn SW1(USER_BUTTON); + +//Use the serial object so we can use higher speeds +Serial terminal(USBTX, USBRX); + +//Timer used for measuring speeds +Timer timer; + +//Enumerated types +enum DIRECTION {FORWARD=0, REVERSE}; +enum PULSE {NOPULSE=0, PULSE}; +enum SWITCHSTATE {PRESSED=0, RELEASED}; + +//Debug GPIO +DigitalOut probe(D10); + +//Duty cycles +float dutyA = 1.0f; //100% +float dutyB = 1.0f; //100% + +//distance measurement +float distanceA ; +float distanceB ; +float speedA ; +float speedB ; +float pretimerA; +float afttimerA; +float pretimerB; +float afttimerB; +float wheel_spacing = 0.128; + +//Completed Loop +int loop=0; + + +void ResetDistanceTimeSpeed() +{ + distanceA=0; + distanceB=0; + speedA=0; + speedB=0; + pretimerA=0; + pretimerB=0; + afttimerA=0; + afttimerB=0; +} + +void stopmotors() +{ + PWMA.write(0.0f); //0% duty cycle + PWMB.write(0.0f); //0% duty cycle +} + +int forward(float distance, float speed, float duty) +{ + //add forward to input with switch for scalability + 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 + ResetDistanceTimeSpeed(); + + //wait for run to complete + while (((distanceA+distanceB)/2) < distance) { + switch { + case(speedA<speed) { + duty += (0.1*duty); + PWMA.write(duty); + } + + case(speedA>speed) { + duty+=(0.1*duty); + PWMA.write(duty); + } + return 1; + } + + int turn(float distance, float duty, int loop) { + //Set initial motor direction + DIRA = FORWARD; + DIRB = FORWARD; + + //test for loop completion + if(loop==1) { + DIRA = REVERSE; + DIRB = REVERSE; + } + /* + 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(0.0f); //Set duty cycle (%) + //reset distance time and speed + resetDistanceTimeSpeed(); + //wait for turn to complete + while (distanceA < distance) { + } + return 1; + } + + void set_distanceA() { + float time = 0; + + afttimerA = timer; //set latest timer to equal timer + distanceA += (wheelc/6); //set distance travelled for this instruction i.e forward/turn etc + terminal.printf("A =%f\n\r",distanceA); // print for fault finding (delete once working) + time = afttimer - pretimer; // + speedA = (wheelc/6)/time); + pretimerA = afttimerA; + } + + void set_distanceB() { + float time = 0; + + afttimerB = timer; + distanceB += (wheelc/6); + terminal.printf("A =%f\n\r",distanceB); + time = afttimer - pretimer; + speedB = (wheelc/6)/time); + pretimerB = afttimerB; + } + + + 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,50); + //terminal.printf("turn\n\r"); + turn(arcl,50,0); + //terminal.printf("forward\n\r"); + forward(6,50); + //terminal.printf("turn\n\r"); + turn(arcl,50,0); + } + + stopmotors(); + + wait(0.5); + + //victory spin + turn(arcl*4.5,50,1); + + stopmotors(); + break; + } + } + } + }
diff -r 000000000000 -r 7118ab174b9c mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Oct 15 12:13:00 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/34e6b704fe68 \ No newline at end of file