Jim Dios / Mbed 2 deprecated MAU2

s

Dependencies:   mbed Motordriver

main.cpp@0:23ce86997770, 2020-03-12 (annotated)

Committer:
jdios
Date:
Thu Mar 12 20:29:39 2020 +0000
Revision:
0:23ce86997770
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jdios 0:23ce86997770 1 #include "mbed.h"
jdios 0:23ce86997770 2 #include "motordriver.h"
jdios 0:23ce86997770 3 #include "math.h"
jdios 0:23ce86997770 4
jdios 0:23ce86997770 5 // Middle = M, Left = L, Right = R
jdios 0:23ce86997770 6
jdios 0:23ce86997770 7 Motor RA(p23, p30, p24, 1);
jdios 0:23ce86997770 8 Motor LA(p22, p29, p24, 1);
jdios 0:23ce86997770 9 Motor MA(p21, p26, p24, 1);
jdios 0:23ce86997770 10
jdios 0:23ce86997770 11 Motor MB(p21, p24, p26, 1);
jdios 0:23ce86997770 12 Motor RB(p23, p24, p30, 1);
jdios 0:23ce86997770 13 Motor LB(p22, p24, p29, 1);
jdios 0:23ce86997770 14
jdios 0:23ce86997770 15 AnalogIn SML(p17); // sensor middle and left above right whell
jdios 0:23ce86997770 16 AnalogIn SMR(p19); // sensor middle and right above left whell
jdios 0:23ce86997770 17 AnalogIn SRL(p20); // sensor right and left above middle whell
jdios 0:23ce86997770 18
jdios 0:23ce86997770 19 void stop()
jdios 0:23ce86997770 20 {
jdios 0:23ce86997770 21 MA.coast();
jdios 0:23ce86997770 22 LA.coast();
jdios 0:23ce86997770 23 RA.coast();
jdios 0:23ce86997770 24 MB.coast();
jdios 0:23ce86997770 25 LB.coast();
jdios 0:23ce86997770 26 RB.coast();
jdios 0:23ce86997770 27 }
jdios 0:23ce86997770 28
jdios 0:23ce86997770 29 float getDistance(float a, float b, float c, float x)
jdios 0:23ce86997770 30 {
jdios 0:23ce86997770 31 return (a - b*x + c*pow(x,2));
jdios 0:23ce86997770 32 }
jdios 0:23ce86997770 33
jdios 0:23ce86997770 34 int main()
jdios 0:23ce86997770 35 {
jdios 0:23ce86997770 36 while(1){
jdios 0:23ce86997770 37
jdios 0:23ce86997770 38 // move back SML - Right
jdios 0:23ce86997770 39 // y = 99.77605 - 382.7846*x + 398.975*x^2
jdios 0:23ce86997770 40 float distanceSML = getDistance(99.77605, 382.7846, 398.975 , SML.read()); // TODO: put parameter for this sensor for a and b
jdios 0:23ce86997770 41 // backSML(0.3, 1000);
jdios 0:23ce86997770 42
jdios 0:23ce86997770 43 // move back SLR - Middle
jdios 0:23ce86997770 44 // y = 99.71885 - 382.2867*x + 398.2079*x^2
jdios 0:23ce86997770 45 float distanceSLR = getDistance(99.71885, 384.0377, 400.3844, SRL.read()); // TODO: put parameter for this sensor for a and b
jdios 0:23ce86997770 46 // backSLR(speedSLR, durationSLR);
jdios 0:23ce86997770 47
jdios 0:23ce86997770 48 // move back SMR - Left
jdios 0:23ce86997770 49 // y = 99.97613 - 384.0377*x + 400.3844*x^2
jdios 0:23ce86997770 50 float distanceSMR = getDistance(99.97613, 384.0377, 400.3844, SMR.read()); // TODO: put parameter for this sensor for a and b
jdios 0:23ce86997770 51 // backSMR(speedSMR, durationSMR);
jdios 0:23ce86997770 52
jdios 0:23ce86997770 53 if(distanceSMR <= 9.5 && distanceSLR <= 9.5 && distanceSML > 9.5){
jdios 0:23ce86997770 54 MB.speed(0.7);
jdios 0:23ce86997770 55 LA.speed(0.7);
jdios 0:23ce86997770 56 RA.speed(0.0);
jdios 0:23ce86997770 57 } else if(distanceSMR > 9.5 && distanceSLR > 9.5 && distanceSML <= 9.5){
jdios 0:23ce86997770 58 MA.speed(0.7);
jdios 0:23ce86997770 59 LB.speed(0.7);
jdios 0:23ce86997770 60 RA.speed(0.0);
jdios 0:23ce86997770 61 }
jdios 0:23ce86997770 62
jdios 0:23ce86997770 63 if(distanceSML <= 9.5 && distanceSLR <= 9.5 && distanceSMR > 9.5 ){
jdios 0:23ce86997770 64 RB.speed(0.7);
jdios 0:23ce86997770 65 MA.speed(0.7);
jdios 0:23ce86997770 66 LA.speed(0.0);
jdios 0:23ce86997770 67 } else if(distanceSML > 9.5 && distanceSLR > 9.5 && distanceSMR <= 9.5 ){
jdios 0:23ce86997770 68 RA.speed(0.7);
jdios 0:23ce86997770 69 MB.speed(0.7);
jdios 0:23ce86997770 70 LA.speed(0.0);
jdios 0:23ce86997770 71 }
jdios 0:23ce86997770 72
jdios 0:23ce86997770 73 if(distanceSML <= 9.5 && distanceSMR <= 9.5 && distanceSLR > 9.5){
jdios 0:23ce86997770 74 LB.speed(0.7);
jdios 0:23ce86997770 75 RA.speed(0.7);
jdios 0:23ce86997770 76 MB.speed(0.0);
jdios 0:23ce86997770 77 } else if(distanceSML > 9.5 && distanceSMR > 9.5 && distanceSLR <= 9.5){
jdios 0:23ce86997770 78 LA.speed(0.7);
jdios 0:23ce86997770 79 RB.speed(0.7);
jdios 0:23ce86997770 80 MB.speed(0.0);
jdios 0:23ce86997770 81 }
jdios 0:23ce86997770 82
jdios 0:23ce86997770 83 // spin
jdios 0:23ce86997770 84
jdios 0:23ce86997770 85 while(distanceSML > 11 && distanceSLR > 11 && distanceSMR > 11) {
jdios 0:23ce86997770 86 RA.speed(0.3);
jdios 0:23ce86997770 87 MA.speed(0.3);
jdios 0:23ce86997770 88 LA.speed(0.3);
jdios 0:23ce86997770 89 // move back SML - Right
jdios 0:23ce86997770 90 // y = 99.77605 - 382.7846*x + 398.975*x^2
jdios 0:23ce86997770 91 float distanceSML = getDistance(99.77605, 382.7846, 398.975 , SML.read()); // TODO: put parameter for this sensor for a and b
jdios 0:23ce86997770 92 // backSML(0.3, 1000);
jdios 0:23ce86997770 93
jdios 0:23ce86997770 94 // move back SLR - Middle
jdios 0:23ce86997770 95 // y = 99.71885 - 382.2867*x + 398.2079*x^2
jdios 0:23ce86997770 96 float distanceSLR = getDistance(99.71885, 384.0377, 400.3844, SRL.read()); // TODO: put parameter for this sensor for a and b
jdios 0:23ce86997770 97 // backSLR(speedSLR, durationSLR);
jdios 0:23ce86997770 98
jdios 0:23ce86997770 99 // move back SMR - Left
jdios 0:23ce86997770 100 // y = 99.97613 - 384.0377*x + 400.3844*x^2
jdios 0:23ce86997770 101 float distanceSMR = getDistance(99.97613, 384.0377, 400.3844, SMR.read()); // TODO: put parameter for this sensor for a and b
jdios 0:23ce86997770 102 // backSMR(speedSMR, durationSMR);
jdios 0:23ce86997770 103
jdios 0:23ce86997770 104 if(distanceSMR <= 9.5 && distanceSLR <= 9.5 && distanceSML > 9.5){
jdios 0:23ce86997770 105 MB.speed(0.7);
jdios 0:23ce86997770 106 LA.speed(0.7);
jdios 0:23ce86997770 107 RA.speed(0.0);
jdios 0:23ce86997770 108 } else if(distanceSMR > 9.5 && distanceSLR > 9.5 && distanceSML <= 9.5){
jdios 0:23ce86997770 109 MA.speed(0.7);
jdios 0:23ce86997770 110 LB.speed(0.7);
jdios 0:23ce86997770 111 RA.speed(0.0);
jdios 0:23ce86997770 112 }
jdios 0:23ce86997770 113
jdios 0:23ce86997770 114 if(distanceSML <= 9.5 && distanceSLR <= 9.5 && distanceSMR > 9.5 ){
jdios 0:23ce86997770 115 RB.speed(0.7);
jdios 0:23ce86997770 116 MA.speed(0.7);
jdios 0:23ce86997770 117 LA.speed(0.0);
jdios 0:23ce86997770 118 } else if(distanceSML > 9.5 && distanceSLR > 9.5 && distanceSMR <= 9.5 ){
jdios 0:23ce86997770 119 RA.speed(0.7);
jdios 0:23ce86997770 120 MB.speed(0.7);
jdios 0:23ce86997770 121 LA.speed(0.0);
jdios 0:23ce86997770 122 }
jdios 0:23ce86997770 123
jdios 0:23ce86997770 124 if(distanceSML <= 9.5 && distanceSMR <= 9.5 && distanceSLR > 9.5){
jdios 0:23ce86997770 125 LB.speed(0.7);
jdios 0:23ce86997770 126 RA.speed(0.7);
jdios 0:23ce86997770 127 MB.speed(0.0);
jdios 0:23ce86997770 128 } else if(distanceSML > 9.5 && distanceSMR > 9.5 && distanceSLR <= 9.5){
jdios 0:23ce86997770 129 LA.speed(0.7);
jdios 0:23ce86997770 130 RB.speed(0.7);
jdios 0:23ce86997770 131 MB.speed(0.0);
jdios 0:23ce86997770 132 }
jdios 0:23ce86997770 133 }
jdios 0:23ce86997770 134
jdios 0:23ce86997770 135
jdios 0:23ce86997770 136 stop();
jdios 0:23ce86997770 137 }
jdios 0:23ce86997770 138 }