![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
speed code
Dependencies: mbed
main.cpp
- Committer:
- benparkes
- Date:
- 2015-10-16
- Revision:
- 0:e79700919e2e
- Child:
- 1:f4e3365155e1
File content as of revision 0:e79700919e2e:
#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; } } } }