nicer code
Dependencies: mbed
Fork of 161015 by
motions.h
- Committer:
- benparkes
- Date:
- 2015-10-18
- Revision:
- 1:f4e3365155e1
File content as of revision 1:f4e3365155e1:
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; }