![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Ready to be uploaded
Dependencies: mbed
Diff: main.cpp
- Revision:
- 0:735ec5eb58d0
diff -r 000000000000 -r 735ec5eb58d0 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Oct 15 13:43:39 2015 +0000 @@ -0,0 +1,168 @@ +#include "mbed.h" + +#define wheelc 0.1759292 // Circumference of Wheel +#define pi 3.141592654 +#define degreel 0.021988888 // Distance Travelled in 1 Degree + + +// 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 wheel_spacing = 0.128; + +// Completed Loop +int loop = 0; + + +void stopmotors() // Stop Motors Spinning +{ + PWMA.write(0.0f); //0% duty cycle + PWMB.write(0.0f); //0% duty cycle +} + +void set_distanceA() +{ + // Hall Effect Measuring Distance for Motor A + distanceA += (wheelc/6); + terminal.printf("A =%f\n\r",distanceA); //Print to terminal +} + +void set_distanceB() +{ + // Hall Effect Measuring Distance for Motor B + distanceB += (wheelc/6); + terminal.printf("B =%f\n\r",distanceB); // Print to Terminal Distance Moved so Far +} + + + +int forward(float distance, float duty) // Move Forwards (distance,speed) +{ + // 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 Total Distance Travelled + distanceA = 0; + distanceB = 0; + timer.reset(); + timer.start(); + + // Wait for Set Distance to be Reached + while (((distanceA+distanceB)/2) < distance) { + } // Using Average Distance for More Accurate Result + 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 + distanceA = 0; + distanceB = 0; + timer.reset(); + timer.start(); + // Wait for Turn to Complete + while (distanceA < distance) { + } + return 1; +} + +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); // Move Forward 1.2m (extra .2 to avoid cones) + turn(90,50,1); // Turn 90° + forward(6,50); // Move Forward .6m (extra .1 to avoid cones) + turn(90,50,1); // Turn 90° + } + + stopmotors(); + wait(0.5); + + //Victory Spin + turn(360,50,0); //Turn 360 degrees in Reverse + + stopmotors(); + break; // End Program, Prevents Looping + } + } + } +} \ No newline at end of file