![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
speed code
Dependencies: mbed
Diff: motions.h
- Revision:
- 1:f4e3365155e1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motions.h Sun Oct 18 18:23:58 2015 +0000 @@ -0,0 +1,82 @@ + +void ResetDistanceTimeSpeed() //reset vaviables for new straight or turn +{ + distanceA=0; + distanceB=0; + speedA=0; + speedB=0; + timer.reset(); + timer.start(); +} + +void stopmotors() //stop motors dead +{ + PWMA.write(0.0f); //0% duty cycle + PWMB.write(0.0f); //0% duty cycle +} + +void HuntSpeeds(float target_speedA, float target_speedB) //alter speeds to desired speed by hunting for corresponding duty (input desired speeds) +{ + + float deltaA = target_speedA -speedA; //find difference between actual speed and desired speed + dutyA = dutyA + deltaA*0.1f; //modify duty dependant upon size of error + PWMA.write(dutyA); //write new duty to motor + + float deltaB = target_speedB -speedB; //repeat for B motor + dutyB = dutyB + deltaB*0.1f; + PWMB.write(dutyB); +} + + +int forward(float distance, float speed) //forward motion, input distance and speed (rev/s) +{ + // Set motor direction forward + DIRA = FORWARD; + DIRB = FORWARD; + + //reset distance + ResetDistanceTimeSpeed(); + + // Set initial motor to be altered by speed code + PWMA.write(0.1f*speed); //Set duty cycle (%) + PWMB.write(0.1*speed); //Set duty cycle (%) + + + //wait for run to complete byy checking if distance has been covered yet + while (((distanceA+distanceB)/2) < distance) { + //maintain desired speed + HuntSpeeds(speed, speed); + } + return 1; +} + +int turn(float degrees, float speed, int direction,float turn_radius) // (Degrees of Turn, Speed, (Anti/Clock)wise, turn radius (m)) + +{ + // Calculate Turn Distance + float C1 = (pi * 2 * turn_radius)*(degrees/360); // outside arc (piD)/fraction turn + float C2 = (pi * 2 * (turn_radius-wheel_spacing))*(degrees/360); //inside arc (piD)/fraction turn + //Set Initial Motor Direction + DIRA = FORWARD; + DIRB = FORWARD; + + // Test for direction to Enter Victory Spin + if(direction==REVERSE) { + DIRA = REVERSE; + DIRB = FORWARD;//victory spin opposite wheels + } + // Reset Distance Travelled + ResetDistanceTimeSpeed(); + + // Wait for Turn to Complete by checking whether outer wheel completed spin yet + while (distanceA < C1) { + if(direction==FORWARD) { + HuntSpeeds(speed, speed*(float(C2/C1))); //maitain speeds proportional to difference in turn length for each wheel + } + + if(direction==REVERSE) { + HuntSpeeds(speed,speed); //set speeds same for reverse spin + } + } + return 1; +} \ No newline at end of file