![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
with speed calc
Dependencies: mbed
main.cpp
- Committer:
- benparkes
- Date:
- 2015-10-15
- Revision:
- 0:7118ab174b9c
File content as of revision 0:7118ab174b9c:
#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; } } } }