Coursework Buggy / Mbed 2 deprecated newcode_noideaifwork

nicer code

Dependencies:   mbed

Fork of 161015 by Coursework Buggy

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?

UserRevisionLine numberNew 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 }