speed code
Dependencies: mbed
motions.h@1:f4e3365155e1, 2015-10-18 (annotated)
- Committer:
- benparkes
- Date:
- Sun Oct 18 18:23:58 2015 +0000
- Revision:
- 1:f4e3365155e1
new code
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
benparkes | 1:f4e3365155e1 | 1 | |
benparkes | 1:f4e3365155e1 | 2 | void ResetDistanceTimeSpeed() //reset vaviables for new straight or turn |
benparkes | 1:f4e3365155e1 | 3 | { |
benparkes | 1:f4e3365155e1 | 4 | distanceA=0; |
benparkes | 1:f4e3365155e1 | 5 | distanceB=0; |
benparkes | 1:f4e3365155e1 | 6 | speedA=0; |
benparkes | 1:f4e3365155e1 | 7 | speedB=0; |
benparkes | 1:f4e3365155e1 | 8 | timer.reset(); |
benparkes | 1:f4e3365155e1 | 9 | timer.start(); |
benparkes | 1:f4e3365155e1 | 10 | } |
benparkes | 1:f4e3365155e1 | 11 | |
benparkes | 1:f4e3365155e1 | 12 | void stopmotors() //stop motors dead |
benparkes | 1:f4e3365155e1 | 13 | { |
benparkes | 1:f4e3365155e1 | 14 | PWMA.write(0.0f); //0% duty cycle |
benparkes | 1:f4e3365155e1 | 15 | PWMB.write(0.0f); //0% duty cycle |
benparkes | 1:f4e3365155e1 | 16 | } |
benparkes | 1:f4e3365155e1 | 17 | |
benparkes | 1:f4e3365155e1 | 18 | void HuntSpeeds(float target_speedA, float target_speedB) //alter speeds to desired speed by hunting for corresponding duty (input desired speeds) |
benparkes | 1:f4e3365155e1 | 19 | { |
benparkes | 1:f4e3365155e1 | 20 | |
benparkes | 1:f4e3365155e1 | 21 | float deltaA = target_speedA -speedA; //find difference between actual speed and desired speed |
benparkes | 1:f4e3365155e1 | 22 | dutyA = dutyA + deltaA*0.1f; //modify duty dependant upon size of error |
benparkes | 1:f4e3365155e1 | 23 | PWMA.write(dutyA); //write new duty to motor |
benparkes | 1:f4e3365155e1 | 24 | |
benparkes | 1:f4e3365155e1 | 25 | float deltaB = target_speedB -speedB; //repeat for B motor |
benparkes | 1:f4e3365155e1 | 26 | dutyB = dutyB + deltaB*0.1f; |
benparkes | 1:f4e3365155e1 | 27 | PWMB.write(dutyB); |
benparkes | 1:f4e3365155e1 | 28 | } |
benparkes | 1:f4e3365155e1 | 29 | |
benparkes | 1:f4e3365155e1 | 30 | |
benparkes | 1:f4e3365155e1 | 31 | int forward(float distance, float speed) //forward motion, input distance and speed (rev/s) |
benparkes | 1:f4e3365155e1 | 32 | { |
benparkes | 1:f4e3365155e1 | 33 | // Set motor direction forward |
benparkes | 1:f4e3365155e1 | 34 | DIRA = FORWARD; |
benparkes | 1:f4e3365155e1 | 35 | DIRB = FORWARD; |
benparkes | 1:f4e3365155e1 | 36 | |
benparkes | 1:f4e3365155e1 | 37 | //reset distance |
benparkes | 1:f4e3365155e1 | 38 | ResetDistanceTimeSpeed(); |
benparkes | 1:f4e3365155e1 | 39 | |
benparkes | 1:f4e3365155e1 | 40 | // Set initial motor to be altered by speed code |
benparkes | 1:f4e3365155e1 | 41 | PWMA.write(0.1f*speed); //Set duty cycle (%) |
benparkes | 1:f4e3365155e1 | 42 | PWMB.write(0.1*speed); //Set duty cycle (%) |
benparkes | 1:f4e3365155e1 | 43 | |
benparkes | 1:f4e3365155e1 | 44 | |
benparkes | 1:f4e3365155e1 | 45 | //wait for run to complete byy checking if distance has been covered yet |
benparkes | 1:f4e3365155e1 | 46 | while (((distanceA+distanceB)/2) < distance) { |
benparkes | 1:f4e3365155e1 | 47 | //maintain desired speed |
benparkes | 1:f4e3365155e1 | 48 | HuntSpeeds(speed, speed); |
benparkes | 1:f4e3365155e1 | 49 | } |
benparkes | 1:f4e3365155e1 | 50 | return 1; |
benparkes | 1:f4e3365155e1 | 51 | } |
benparkes | 1:f4e3365155e1 | 52 | |
benparkes | 1:f4e3365155e1 | 53 | int turn(float degrees, float speed, int direction,float turn_radius) // (Degrees of Turn, Speed, (Anti/Clock)wise, turn radius (m)) |
benparkes | 1:f4e3365155e1 | 54 | |
benparkes | 1:f4e3365155e1 | 55 | { |
benparkes | 1:f4e3365155e1 | 56 | // Calculate Turn Distance |
benparkes | 1:f4e3365155e1 | 57 | float C1 = (pi * 2 * turn_radius)*(degrees/360); // outside arc (piD)/fraction turn |
benparkes | 1:f4e3365155e1 | 58 | float C2 = (pi * 2 * (turn_radius-wheel_spacing))*(degrees/360); //inside arc (piD)/fraction turn |
benparkes | 1:f4e3365155e1 | 59 | //Set Initial Motor Direction |
benparkes | 1:f4e3365155e1 | 60 | DIRA = FORWARD; |
benparkes | 1:f4e3365155e1 | 61 | DIRB = FORWARD; |
benparkes | 1:f4e3365155e1 | 62 | |
benparkes | 1:f4e3365155e1 | 63 | // Test for direction to Enter Victory Spin |
benparkes | 1:f4e3365155e1 | 64 | if(direction==REVERSE) { |
benparkes | 1:f4e3365155e1 | 65 | DIRA = REVERSE; |
benparkes | 1:f4e3365155e1 | 66 | DIRB = FORWARD;//victory spin opposite wheels |
benparkes | 1:f4e3365155e1 | 67 | } |
benparkes | 1:f4e3365155e1 | 68 | // Reset Distance Travelled |
benparkes | 1:f4e3365155e1 | 69 | ResetDistanceTimeSpeed(); |
benparkes | 1:f4e3365155e1 | 70 | |
benparkes | 1:f4e3365155e1 | 71 | // Wait for Turn to Complete by checking whether outer wheel completed spin yet |
benparkes | 1:f4e3365155e1 | 72 | while (distanceA < C1) { |
benparkes | 1:f4e3365155e1 | 73 | if(direction==FORWARD) { |
benparkes | 1:f4e3365155e1 | 74 | HuntSpeeds(speed, speed*(float(C2/C1))); //maitain speeds proportional to difference in turn length for each wheel |
benparkes | 1:f4e3365155e1 | 75 | } |
benparkes | 1:f4e3365155e1 | 76 | |
benparkes | 1:f4e3365155e1 | 77 | if(direction==REVERSE) { |
benparkes | 1:f4e3365155e1 | 78 | HuntSpeeds(speed,speed); //set speeds same for reverse spin |
benparkes | 1:f4e3365155e1 | 79 | } |
benparkes | 1:f4e3365155e1 | 80 | } |
benparkes | 1:f4e3365155e1 | 81 | return 1; |
benparkes | 1:f4e3365155e1 | 82 | } |