Antoine Laurens / Mbed 2 deprecated STRAIGHT_DRIVE_NO_SEP

drive down

Dependencies:   BMP280 BNO055_fusion PowerControl mbed

Fork of TEAM_G_FLOW_RIDA by Edwin Cho

LOCOMOTION.cpp@24:fb1f083ebd62, 2016-04-03 (annotated)

Committer:
12104404
Date:
Sun Apr 03 07:25:07 2016 +0000
Revision:
24:fb1f083ebd62
Parent:
23:455f7da3dd7a
Child:
25:f3bf8351bbd4
Added LED Error Indicators

Who changed what in which revision?

UserRevisionLine numberNew contents of line
12104404 6:0602a9e8118b 1 #include "LOCOMOTION.h"
12104404 15:7729da55873a 2
12104404 24:fb1f083ebd62 3 LOCOMOTION::LOCOMOTION (PinName en, PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4):
12104404 24:fb1f083ebd62 4 _en(en), _m1f(motor1F), _m1b(motor1B), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2), _led1(led1), _led2(led2), _led3(led3), _led4(led4)
12104404 23:455f7da3dd7a 5 {
12104404 23:455f7da3dd7a 6 s=0;
12104404 23:455f7da3dd7a 7 stopMotors();
12104404 23:455f7da3dd7a 8 _m1dir=0;
12104404 23:455f7da3dd7a 9 _m2dir=0;
12104404 23:455f7da3dd7a 10 _en=1;
12104404 24:fb1f083ebd62 11 _led1=1;
12104404 24:fb1f083ebd62 12 wait(0.01);
12104404 24:fb1f083ebd62 13 _led1=0;
12104404 23:455f7da3dd7a 14 }
12104404 23:455f7da3dd7a 15
12104404 23:455f7da3dd7a 16 inline void LOCOMOTION::stopMotors(void)
12104404 15:7729da55873a 17 {
12104404 15:7729da55873a 18 _m1f=0;
12104404 15:7729da55873a 19 _m1b=0;
12104404 15:7729da55873a 20 _m2f=0;
12104404 15:7729da55873a 21 _m2b=0;
12104404 15:7729da55873a 22 }
12104404 15:7729da55873a 23
12104404 23:455f7da3dd7a 24
12104404 23:455f7da3dd7a 25 inline int LOCOMOTION::wrap(int num)
12104404 19:2dd81b864e14 26 {
12104404 23:455f7da3dd7a 27 return num%360;
12104404 23:455f7da3dd7a 28 }
12104404 23:455f7da3dd7a 29
12104404 23:455f7da3dd7a 30 void LOCOMOTION::eStop(void)
12104404 23:455f7da3dd7a 31 {
12104404 23:455f7da3dd7a 32 //Stop Motors
12104404 23:455f7da3dd7a 33 stopMotors();
12104404 23:455f7da3dd7a 34 //Disable Motor Drivers
12104404 23:455f7da3dd7a 35 _en=0;
12104404 19:2dd81b864e14 36 }
12104404 19:2dd81b864e14 37
12104404 18:f9012e93edb8 38 bool LOCOMOTION::setXPos(int target, int current, int error, int angle)
12104404 17:2f89826b5679 39 {
12104404 17:2f89826b5679 40 //s = 0;
12104404 17:2f89826b5679 41 if(abs(current-target)<=error)
12104404 17:2f89826b5679 42 s=0.07;
12104404 17:2f89826b5679 43 else
12104404 17:2f89826b5679 44 s=((0.17-0.07)*abs(current-target)/FRAME_W)+0.07;
12104404 17:2f89826b5679 45 if(current>target+error) {
12104404 18:f9012e93edb8 46 if(angle==0) {
12104404 18:f9012e93edb8 47 _m1dir=1;
12104404 18:f9012e93edb8 48 _m2dir=1;
12104404 18:f9012e93edb8 49 } else {
12104404 18:f9012e93edb8 50 _m1dir=0;
12104404 18:f9012e93edb8 51 _m2dir=0;
12104404 18:f9012e93edb8 52 }
12104404 17:2f89826b5679 53 _m1f=s;
12104404 17:2f89826b5679 54 _m1b=s;
12104404 17:2f89826b5679 55 _m2f=s;
12104404 17:2f89826b5679 56 _m2b=s;
12104404 17:2f89826b5679 57 } else if(current<target-error) {
12104404 18:f9012e93edb8 58 if(angle==0) {
12104404 18:f9012e93edb8 59 _m1dir=0;
12104404 18:f9012e93edb8 60 _m2dir=0;
12104404 18:f9012e93edb8 61 } else {
12104404 18:f9012e93edb8 62 _m1dir=1;
12104404 18:f9012e93edb8 63 _m2dir=1;
12104404 18:f9012e93edb8 64 }
12104404 17:2f89826b5679 65 _m1f=s;
12104404 17:2f89826b5679 66 _m1b=s;
12104404 17:2f89826b5679 67 _m2f=s;
12104404 17:2f89826b5679 68 _m2b=s;
12104404 17:2f89826b5679 69 } else {
12104404 19:2dd81b864e14 70 stopMotors();
12104404 17:2f89826b5679 71 return true;
12104404 17:2f89826b5679 72 }
12104404 17:2f89826b5679 73 return false;
12104404 17:2f89826b5679 74 }
12104404 17:2f89826b5679 75
12104404 18:f9012e93edb8 76 bool LOCOMOTION::setYPos(int target, int current, int error, int angle)
12104404 17:2f89826b5679 77 {
12104404 17:2f89826b5679 78 //float s = 0;
12104404 17:2f89826b5679 79 if(abs(current-target)<=error)
12104404 17:2f89826b5679 80 s=0.50;
12104404 17:2f89826b5679 81 else
12104404 17:2f89826b5679 82 s=((1-0.50)*abs(current-target)/FRAME_H)+0.50;
12104404 17:2f89826b5679 83 if(current>target+error) {
12104404 17:2f89826b5679 84 //_m1dir=1;
12104404 17:2f89826b5679 85 //_m2dir=1;
12104404 18:f9012e93edb8 86 if(angle==0) {
12104404 18:f9012e93edb8 87 _m1f=_m1f*(1+s);
12104404 18:f9012e93edb8 88 _m1b=_m1b*(1+s);
12104404 18:f9012e93edb8 89 } else {
12104404 18:f9012e93edb8 90 _m2f=_m2f*(1+s);
12104404 18:f9012e93edb8 91 _m2b=_m2b*(1+s);
12104404 18:f9012e93edb8 92 }
12104404 17:2f89826b5679 93 } else if(current<target-error) {
12104404 17:2f89826b5679 94 //_m1dir=0;
12104404 17:2f89826b5679 95 //_m2dir=0;
12104404 18:f9012e93edb8 96 if(angle==0) {
12104404 18:f9012e93edb8 97 _m2f=_m2f*(1+s);
12104404 18:f9012e93edb8 98 _m2b=_m2b*(1+s);
12104404 18:f9012e93edb8 99 } else {
12104404 18:f9012e93edb8 100 _m1f=_m1f*(1+s);
12104404 18:f9012e93edb8 101 _m1b=_m1b*(1+s);
12104404 18:f9012e93edb8 102 }
12104404 17:2f89826b5679 103 } else {
12104404 17:2f89826b5679 104 s=0;
12104404 18:f9012e93edb8 105
12104404 17:2f89826b5679 106 return true;
12104404 17:2f89826b5679 107 }
12104404 17:2f89826b5679 108 return false;
12104404 17:2f89826b5679 109 }
12104404 17:2f89826b5679 110
12104404 15:7729da55873a 111 bool LOCOMOTION::setAngle(int target, int current, int error, int mode)
12104404 15:7729da55873a 112 {
12104404 17:2f89826b5679 113 s = 0;
12104404 15:7729da55873a 114 int diff = 0;
12104404 15:7729da55873a 115 diff = 180-wrap(target);
12104404 15:7729da55873a 116 if(abs(wrap(current+diff)-180)<=error)
12104404 15:7729da55873a 117 s=SPEED_TURN_MIN;
12104404 15:7729da55873a 118 else
12104404 15:7729da55873a 119 s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN;
12104404 15:7729da55873a 120 switch(mode) {
12104404 15:7729da55873a 121 case ANGLE_TURN:
12104404 16:d6f15a13c3aa 122 if(wrap(current+diff)>180+error) {
12104404 16:d6f15a13c3aa 123 _m1dir=1;
12104404 16:d6f15a13c3aa 124 _m2dir=0;
12104404 16:d6f15a13c3aa 125 _m1f=s;
12104404 16:d6f15a13c3aa 126 _m1b=s;
12104404 16:d6f15a13c3aa 127 _m2f=s;
12104404 16:d6f15a13c3aa 128 _m2b=s;
12104404 16:d6f15a13c3aa 129 } else if(wrap(current+diff)<180-error) {
12104404 16:d6f15a13c3aa 130 _m1dir=0;
12104404 16:d6f15a13c3aa 131 _m2dir=1;
12104404 16:d6f15a13c3aa 132 _m1f=s;
12104404 16:d6f15a13c3aa 133 _m1b=s;
12104404 16:d6f15a13c3aa 134 _m2f=s;
12104404 16:d6f15a13c3aa 135 _m2b=s;
12104404 16:d6f15a13c3aa 136 } else {
12104404 19:2dd81b864e14 137 stopMotors();
12104404 16:d6f15a13c3aa 138 return true;
12104404 16:d6f15a13c3aa 139 }
12104404 15:7729da55873a 140 break;
12104404 15:7729da55873a 141 case ANGLE_BIAS:
12104404 15:7729da55873a 142
12104404 15:7729da55873a 143 break;
12104404 15:7729da55873a 144 }
12104404 16:d6f15a13c3aa 145 return false;
12104404 23:455f7da3dd7a 146 }