s
Dependencies: mbed Motordriver
main.cpp
- Committer:
- jdios
- Date:
- 2020-03-12
- Revision:
- 0:23ce86997770
File content as of revision 0:23ce86997770:
#include "mbed.h" #include "motordriver.h" #include "math.h" // Middle = M, Left = L, Right = R Motor RA(p23, p30, p24, 1); Motor LA(p22, p29, p24, 1); Motor MA(p21, p26, p24, 1); Motor MB(p21, p24, p26, 1); Motor RB(p23, p24, p30, 1); Motor LB(p22, p24, p29, 1); AnalogIn SML(p17); // sensor middle and left above right whell AnalogIn SMR(p19); // sensor middle and right above left whell AnalogIn SRL(p20); // sensor right and left above middle whell void stop() { MA.coast(); LA.coast(); RA.coast(); MB.coast(); LB.coast(); RB.coast(); } float getDistance(float a, float b, float c, float x) { return (a - b*x + c*pow(x,2)); } int main() { while(1){ // move back SML - Right // y = 99.77605 - 382.7846*x + 398.975*x^2 float distanceSML = getDistance(99.77605, 382.7846, 398.975 , SML.read()); // TODO: put parameter for this sensor for a and b // backSML(0.3, 1000); // move back SLR - Middle // y = 99.71885 - 382.2867*x + 398.2079*x^2 float distanceSLR = getDistance(99.71885, 384.0377, 400.3844, SRL.read()); // TODO: put parameter for this sensor for a and b // backSLR(speedSLR, durationSLR); // move back SMR - Left // y = 99.97613 - 384.0377*x + 400.3844*x^2 float distanceSMR = getDistance(99.97613, 384.0377, 400.3844, SMR.read()); // TODO: put parameter for this sensor for a and b // backSMR(speedSMR, durationSMR); if(distanceSMR <= 9.5 && distanceSLR <= 9.5 && distanceSML > 9.5){ MB.speed(0.7); LA.speed(0.7); RA.speed(0.0); } else if(distanceSMR > 9.5 && distanceSLR > 9.5 && distanceSML <= 9.5){ MA.speed(0.7); LB.speed(0.7); RA.speed(0.0); } if(distanceSML <= 9.5 && distanceSLR <= 9.5 && distanceSMR > 9.5 ){ RB.speed(0.7); MA.speed(0.7); LA.speed(0.0); } else if(distanceSML > 9.5 && distanceSLR > 9.5 && distanceSMR <= 9.5 ){ RA.speed(0.7); MB.speed(0.7); LA.speed(0.0); } if(distanceSML <= 9.5 && distanceSMR <= 9.5 && distanceSLR > 9.5){ LB.speed(0.7); RA.speed(0.7); MB.speed(0.0); } else if(distanceSML > 9.5 && distanceSMR > 9.5 && distanceSLR <= 9.5){ LA.speed(0.7); RB.speed(0.7); MB.speed(0.0); } // spin while(distanceSML > 11 && distanceSLR > 11 && distanceSMR > 11) { RA.speed(0.3); MA.speed(0.3); LA.speed(0.3); // move back SML - Right // y = 99.77605 - 382.7846*x + 398.975*x^2 float distanceSML = getDistance(99.77605, 382.7846, 398.975 , SML.read()); // TODO: put parameter for this sensor for a and b // backSML(0.3, 1000); // move back SLR - Middle // y = 99.71885 - 382.2867*x + 398.2079*x^2 float distanceSLR = getDistance(99.71885, 384.0377, 400.3844, SRL.read()); // TODO: put parameter for this sensor for a and b // backSLR(speedSLR, durationSLR); // move back SMR - Left // y = 99.97613 - 384.0377*x + 400.3844*x^2 float distanceSMR = getDistance(99.97613, 384.0377, 400.3844, SMR.read()); // TODO: put parameter for this sensor for a and b // backSMR(speedSMR, durationSMR); if(distanceSMR <= 9.5 && distanceSLR <= 9.5 && distanceSML > 9.5){ MB.speed(0.7); LA.speed(0.7); RA.speed(0.0); } else if(distanceSMR > 9.5 && distanceSLR > 9.5 && distanceSML <= 9.5){ MA.speed(0.7); LB.speed(0.7); RA.speed(0.0); } if(distanceSML <= 9.5 && distanceSLR <= 9.5 && distanceSMR > 9.5 ){ RB.speed(0.7); MA.speed(0.7); LA.speed(0.0); } else if(distanceSML > 9.5 && distanceSLR > 9.5 && distanceSMR <= 9.5 ){ RA.speed(0.7); MB.speed(0.7); LA.speed(0.0); } if(distanceSML <= 9.5 && distanceSMR <= 9.5 && distanceSLR > 9.5){ LB.speed(0.7); RA.speed(0.7); MB.speed(0.0); } else if(distanceSML > 9.5 && distanceSMR > 9.5 && distanceSLR <= 9.5){ LA.speed(0.7); RB.speed(0.7); MB.speed(0.0); } } stop(); } }