speed code
Dependencies: mbed
Diff: main.cpp
- Revision:
- 0:e79700919e2e
- Child:
- 1:f4e3365155e1
diff -r 000000000000 -r e79700919e2e main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Oct 16 12:39:12 2015 +0000 @@ -0,0 +1,213 @@ +#include "mbed.h" + +#define wheelc 0.1759292 +#define pi 3.141592654 +#define degreel 0.021988888 +//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.000f; //100% +float dutyB = 1.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; + +//Completed Loop +int loop=0; + +int turn(); +void ResetDistanceTimeSpeed() +{ + distanceA=0; + distanceB=0; + speedA=0; + speedB=0; + pretimerA=0; + pretimerB=0; + afttimerA=0; + afttimerB=0; + timer.reset(); + timer.start(); +} + +void stopmotors() +{ + PWMA.write(0.0f); //0% duty cycle + PWMB.write(0.0f); //0% duty cycle +} + +int forward(float distance, float speed) +{ + //add forward to input with switch for scalability + // Set motor direction forward + DIRA = FORWARD; + DIRB = FORWARD; + + //reset distance + ResetDistanceTimeSpeed(); + + // Set motor speed to input speed + PWMA.write(0.1); //Set duty cycle (%) + PWMB.write(0.1); //Set duty cycle (%) + + + //wait for run to complete + 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) { + } + return 1; +} + + void set_distanceA() { + float time = 0; + + 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); + } + + void set_distanceB() { + float time = 0; + + 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); + + 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; + } + } + } + + } +