Kaiwalya Belsare / Mbed 2 deprecated TennisBall_PickingRobot

Code

Dependencies:   mbed QEI MPU6050 TextLCD

main.cpp@5:84e63108a1b6, 2020-03-11 (annotated)

Committer:
belsarekaiwalya
Date:
Wed Mar 11 10:02:43 2020 +0000
Revision:
5:84e63108a1b6
Parent:
4:b084af72f9a6 
h

Who changed what in which revision?

UserRevisionLine numberNew contents of line
belsarekaiwalya 0:c9c8c027609e 1 #include "mbed.h"
belsarekaiwalya 0:c9c8c027609e 2 #include "TextLCD.h"
belsarekaiwalya 5:84e63108a1b6 3 #include "QEI.h"
belsarekaiwalya 5:84e63108a1b6 4
belsarekaiwalya 5:84e63108a1b6 5 #define ppr 1120
belsarekaiwalya 5:84e63108a1b6 6 #define diam 11.0
belsarekaiwalya 5:84e63108a1b6 7 #define circum (3.14*diam)
belsarekaiwalya 5:84e63108a1b6 8 #define width 23.0
belsarekaiwalya 5:84e63108a1b6 9 #define motRPM 150
belsarekaiwalya 5:84e63108a1b6 10
belsarekaiwalya 0:c9c8c027609e 11 TextLCD lcd(PA_0,PA_1,PA_4,PB_0,PC_1,PC_0); // rs, e, d4-d7
belsarekaiwalya 5:84e63108a1b6 12 QEI leftEnc(PB_8,PB_9);
belsarekaiwalya 5:84e63108a1b6 13 QEI rightEnc(PA_6,PA_5);
belsarekaiwalya 5:84e63108a1b6 14
belsarekaiwalya 3:86145fd89b45 15 Serial serial(USBTX, USBRX);
belsarekaiwalya 5:84e63108a1b6 16 //Serial hmcRead(PC_10,PC_11);//tx rx
belsarekaiwalya 5:84e63108a1b6 17 Serial bt(PA_9,PA_10);//tx,rx
belsarekaiwalya 5:84e63108a1b6 18 Serial xbee1(PC_10,PC_11);
belsarekaiwalya 5:84e63108a1b6 19
belsarekaiwalya 1:59c416ccba42 20 DigitalOut dirLa(PB_4);
belsarekaiwalya 1:59c416ccba42 21 DigitalOut dirLb(PB_10);
belsarekaiwalya 5:84e63108a1b6 22 PwmOut pwmR(PA_8);//left
belsarekaiwalya 0:c9c8c027609e 23
belsarekaiwalya 5:84e63108a1b6 24 DigitalOut dirRa(PB_5);
belsarekaiwalya 5:84e63108a1b6 25 DigitalOut dirRb(PB_3);
belsarekaiwalya 5:84e63108a1b6 26 PwmOut pwmL(PC_7);//right
belsarekaiwalya 5:84e63108a1b6 27 Ticker TISR;
belsarekaiwalya 5:84e63108a1b6 28 Ticker Print;
belsarekaiwalya 5:84e63108a1b6 29
belsarekaiwalya 5:84e63108a1b6 30 DigitalOut rolA(PA_13);
belsarekaiwalya 5:84e63108a1b6 31 DigitalOut rolB(PA_14);
belsarekaiwalya 5:84e63108a1b6 32 PwmOut RolPwm(PA_15);//roller
belsarekaiwalya 5:84e63108a1b6 33
belsarekaiwalya 5:84e63108a1b6 34 void rolStop();
belsarekaiwalya 3:86145fd89b45 35 void InitSerial();
belsarekaiwalya 4:b084af72f9a6 36 void runMotor(char mot, bool dir, float pwm);
belsarekaiwalya 5:84e63108a1b6 37 float updatePIDL();
belsarekaiwalya 5:84e63108a1b6 38 float updatePIDR();
belsarekaiwalya 5:84e63108a1b6 39 void keepEnCount(void);
belsarekaiwalya 5:84e63108a1b6 40 void printData(void);
belsarekaiwalya 5:84e63108a1b6 41 void execPIDL();
belsarekaiwalya 5:84e63108a1b6 42 void execPIDR();
belsarekaiwalya 5:84e63108a1b6 43 void hmcCorrect();
belsarekaiwalya 5:84e63108a1b6 44 float updatePID();double hoq;
belsarekaiwalya 5:84e63108a1b6 45 void allfunction();
belsarekaiwalya 5:84e63108a1b6 46 void runRol();
belsarekaiwalya 3:86145fd89b45 47 void Brake();
belsarekaiwalya 5:84e63108a1b6 48 float err;
belsarekaiwalya 3:86145fd89b45 49 int ch;
belsarekaiwalya 5:84e63108a1b6 50 char run_mode;
belsarekaiwalya 5:84e63108a1b6 51 bool zom = true;int jim=0,jim1=0;
belsarekaiwalya 5:84e63108a1b6 52 long Count1,Count2;
belsarekaiwalya 5:84e63108a1b6 53 long CountL,CountR,CountDiffL,CountDiffR,CountPrevL,CountPrevR;
belsarekaiwalya 5:84e63108a1b6 54 float correctPwmL,correctPwmR;
belsarekaiwalya 5:84e63108a1b6 55 double distL,distR,distAv,velL,velR;
belsarekaiwalya 5:84e63108a1b6 56 float xpwmL=0.8*1.1,xpwmR=0.8;
belsarekaiwalya 5:84e63108a1b6 57 float xpwmLc,xpwmRc,xpwmLR,outputPID;
belsarekaiwalya 5:84e63108a1b6 58 double KpL=0.1,KpR=0.125,KdL=0,KdR=0,Kp=0.3,Kd = 0;
belsarekaiwalya 5:84e63108a1b6 59 //int Mx,Mx1,err,lastError;
belsarekaiwalya 5:84e63108a1b6 60 //float outputPID,P,D;
belsarekaiwalya 3:86145fd89b45 61 void InitSerial()
belsarekaiwalya 0:c9c8c027609e 62 {
belsarekaiwalya 0:c9c8c027609e 63 bt.baud(9600);
belsarekaiwalya 0:c9c8c027609e 64 serial.baud(9600);
belsarekaiwalya 3:86145fd89b45 65 }
belsarekaiwalya 3:86145fd89b45 66 void SetPwmf_kHz(float freq)
belsarekaiwalya 3:86145fd89b45 67 {
belsarekaiwalya 5:84e63108a1b6 68
belsarekaiwalya 5:84e63108a1b6 69 pwmL.period_ms(1/freq);
belsarekaiwalya 5:84e63108a1b6 70 pwmR.period_ms(1/freq);
belsarekaiwalya 5:84e63108a1b6 71 RolPwm.period_ms(1/freq);
belsarekaiwalya 5:84e63108a1b6 72 }
belsarekaiwalya 5:84e63108a1b6 73 /*void hmcCorrect()
belsarekaiwalya 5:84e63108a1b6 74 {
belsarekaiwalya 5:84e63108a1b6 75 err = Mx1 - Mx;
belsarekaiwalya 5:84e63108a1b6 76 P = err*KpL;
belsarekaiwalya 5:84e63108a1b6 77 D = (err - lastError)*KdL;
belsarekaiwalya 5:84e63108a1b6 78 outputPID = (P + D);
belsarekaiwalya 5:84e63108a1b6 79 lastError = err;
belsarekaiwalya 5:84e63108a1b6 80 if(outputPID < 0)
belsarekaiwalya 5:84e63108a1b6 81 {runMotor('L', 1, (xpwmL + outputPID));
belsarekaiwalya 5:84e63108a1b6 82 runMotor('R', 1, (xpwmR - outputPID));}
belsarekaiwalya 5:84e63108a1b6 83 else if(outputPID > 0)
belsarekaiwalya 5:84e63108a1b6 84 {runMotor('L', 1, (xpwmL + outputPID));
belsarekaiwalya 5:84e63108a1b6 85 runMotor('R', 1, (xpwmR - outputPID));}
belsarekaiwalya 5:84e63108a1b6 86 else
belsarekaiwalya 5:84e63108a1b6 87 {runMotor('L', 1, (xpwmL));
belsarekaiwalya 5:84e63108a1b6 88 runMotor('R', 1, (xpwmR));}
belsarekaiwalya 5:84e63108a1b6 89 }*/
belsarekaiwalya 5:84e63108a1b6 90 void printData()
belsarekaiwalya 5:84e63108a1b6 91 {
belsarekaiwalya 5:84e63108a1b6 92 // lcd.cls();
belsarekaiwalya 5:84e63108a1b6 93 // lcd.locate(1,0);
belsarekaiwalya 5:84e63108a1b6 94 //lcd.printf("%f",outputPID );
belsarekaiwalya 5:84e63108a1b6 95 //lcd.locate(1,1);
belsarekaiwalya 5:84e63108a1b6 96 //lcd.printf("%f",velR);
belsarekaiwalya 5:84e63108a1b6 97 }
belsarekaiwalya 3:86145fd89b45 98 int main()
belsarekaiwalya 3:86145fd89b45 99 {
belsarekaiwalya 3:86145fd89b45 100 InitSerial();
belsarekaiwalya 3:86145fd89b45 101 SetPwmf_kHz(1);
belsarekaiwalya 5:84e63108a1b6 102 TISR.attach(&keepEnCount, 0.01);
belsarekaiwalya 5:84e63108a1b6 103 Print.attach(&printData, 0.05);
belsarekaiwalya 5:84e63108a1b6 104 lcd.cls();
belsarekaiwalya 3:86145fd89b45 105 Brake();
belsarekaiwalya 5:84e63108a1b6 106
belsarekaiwalya 0:c9c8c027609e 107 while(1)
belsarekaiwalya 0:c9c8c027609e 108 {
belsarekaiwalya 5:84e63108a1b6 109
belsarekaiwalya 5:84e63108a1b6 110 //rolA=1;rolB=0;
belsarekaiwalya 5:84e63108a1b6 111 /* CountL = leftEnc.read();
belsarekaiwalya 5:84e63108a1b6 112 CountR = rightEnc.read();
belsarekaiwalya 5:84e63108a1b6 113 distL = (CountL/ppr)*circum;
belsarekaiwalya 5:84e63108a1b6 114 distR = (CountR/ppr)*circum;
belsarekaiwalya 5:84e63108a1b6 115 distAv = (0.5*(distL + distR));
belsarekaiwalya 5:84e63108a1b6 116 xpwmLc = float(CountDiffL*60)/(ppr*motRPM);
belsarekaiwalya 5:84e63108a1b6 117 xpwmRc = float(CountDiffR*60)/(ppr*motRPM);
belsarekaiwalya 5:84e63108a1b6 118 xpwmLR = float((CountDiffL-CountDiffR)*60)/(ppr*motRPM);
belsarekaiwalya 5:84e63108a1b6 119 velL = (circum*motRPM*xpwmLc)/60;
belsarekaiwalya 5:84e63108a1b6 120 velR = (circum*motRPM*xpwmRc)/60;
belsarekaiwalya 5:84e63108a1b6 121 correctPwmL = updatePIDL();
belsarekaiwalya 5:84e63108a1b6 122 correctPwmR = updatePIDR();
belsarekaiwalya 5:84e63108a1b6 123 outputPID = updatePID();
belsarekaiwalya 5:84e63108a1b6 124 if(outputPID < 0)
belsarekaiwalya 5:84e63108a1b6 125 {runMotor('L', 1, (xpwmL - outputPID));
belsarekaiwalya 5:84e63108a1b6 126 runMotor('R', 1, (xpwmR + outputPID));}
belsarekaiwalya 5:84e63108a1b6 127 else if(outputPID > 0)
belsarekaiwalya 5:84e63108a1b6 128 {runMotor('L', 1, (xpwmL - outputPID));
belsarekaiwalya 5:84e63108a1b6 129 runMotor('R', 1, (xpwmR + outputPID));}
belsarekaiwalya 5:84e63108a1b6 130 else
belsarekaiwalya 5:84e63108a1b6 131 {runMotor('L', 1, (xpwmL));
belsarekaiwalya 5:84e63108a1b6 132 runMotor('R', 1, (xpwmR));}
belsarekaiwalya 5:84e63108a1b6 133
belsarekaiwalya 5:84e63108a1b6 134 //execPIDL();
belsarekaiwalya 5:84e63108a1b6 135 //execPIDR();
belsarekaiwalya 5:84e63108a1b6 136
belsarekaiwalya 5:84e63108a1b6 137
belsarekaiwalya 5:84e63108a1b6 138 */
belsarekaiwalya 5:84e63108a1b6 139 CountL = leftEnc.read();
belsarekaiwalya 5:84e63108a1b6 140 CountR = rightEnc.read();
belsarekaiwalya 5:84e63108a1b6 141 distL = (CountL/ppr)*circum;
belsarekaiwalya 5:84e63108a1b6 142 distR = (CountR/ppr)*circum;
belsarekaiwalya 5:84e63108a1b6 143 distAv = (0.5*(distL + distR));
belsarekaiwalya 5:84e63108a1b6 144 xpwmLc = float(CountDiffL*60)/(ppr*motRPM);
belsarekaiwalya 5:84e63108a1b6 145 xpwmRc = float(CountDiffR*60)/(ppr*motRPM);
belsarekaiwalya 5:84e63108a1b6 146 xpwmLR = float((CountDiffL-CountDiffR)*60)/(ppr*motRPM);
belsarekaiwalya 5:84e63108a1b6 147 velL = (circum*motRPM*xpwmLc)/60;
belsarekaiwalya 5:84e63108a1b6 148 velR = (circum*motRPM*xpwmRc)/60;
belsarekaiwalya 5:84e63108a1b6 149 correctPwmL = updatePIDL();
belsarekaiwalya 5:84e63108a1b6 150 correctPwmR = updatePIDR();
belsarekaiwalya 5:84e63108a1b6 151 //RolPwm = 0.5;
belsarekaiwalya 5:84e63108a1b6 152
belsarekaiwalya 5:84e63108a1b6 153 if(xbee1.readable())
belsarekaiwalya 0:c9c8c027609e 154 {
belsarekaiwalya 5:84e63108a1b6 155 run_mode = xbee1.getc();
belsarekaiwalya 5:84e63108a1b6 156 //serial.printf("%c\n",run_mode);
belsarekaiwalya 5:84e63108a1b6 157 switch(run_mode)
belsarekaiwalya 0:c9c8c027609e 158 {
belsarekaiwalya 5:84e63108a1b6 159 case 'd':
belsarekaiwalya 5:84e63108a1b6 160 runMotor('L', 1, 0.7);
belsarekaiwalya 5:84e63108a1b6 161 runMotor('R', 1, 0.65);
belsarekaiwalya 5:84e63108a1b6 162 serial.printf("M Forward\n");
belsarekaiwalya 5:84e63108a1b6 163 break;//front
belsarekaiwalya 5:84e63108a1b6 164
belsarekaiwalya 5:84e63108a1b6 165 case 'a':
belsarekaiwalya 5:84e63108a1b6 166 runMotor('L', 1, 1);
belsarekaiwalya 5:84e63108a1b6 167 runMotor('R', 0, 1);
belsarekaiwalya 5:84e63108a1b6 168 serial.printf("M Right\n");
belsarekaiwalya 5:84e63108a1b6 169 break;//right
belsarekaiwalya 5:84e63108a1b6 170
belsarekaiwalya 5:84e63108a1b6 171 case 's':
belsarekaiwalya 5:84e63108a1b6 172 runMotor('L', 0, 1);
belsarekaiwalya 5:84e63108a1b6 173 runMotor('R', 0, 1);
belsarekaiwalya 5:84e63108a1b6 174 serial.printf("M Back\n");
belsarekaiwalya 5:84e63108a1b6 175 break;//back
belsarekaiwalya 5:84e63108a1b6 176
belsarekaiwalya 5:84e63108a1b6 177 case 'w':
belsarekaiwalya 5:84e63108a1b6 178 runMotor('L', 0, 1);
belsarekaiwalya 5:84e63108a1b6 179 runMotor('R', 1, 1);
belsarekaiwalya 5:84e63108a1b6 180 serial.printf("M Left\n");
belsarekaiwalya 5:84e63108a1b6 181 break;//left
belsarekaiwalya 5:84e63108a1b6 182
belsarekaiwalya 5:84e63108a1b6 183 case 'x':
belsarekaiwalya 5:84e63108a1b6 184 runMotor('L', 0, 0);
belsarekaiwalya 5:84e63108a1b6 185 runMotor('R', 1, 0);
belsarekaiwalya 5:84e63108a1b6 186 serial.printf("M Stop\n");
belsarekaiwalya 5:84e63108a1b6 187 break;//stop
belsarekaiwalya 5:84e63108a1b6 188 }
belsarekaiwalya 5:84e63108a1b6 189 }
belsarekaiwalya 5:84e63108a1b6 190 else if(bt.readable())
belsarekaiwalya 5:84e63108a1b6 191 {
belsarekaiwalya 5:84e63108a1b6 192 ch = bt.getc();
belsarekaiwalya 5:84e63108a1b6 193 serial.printf("%c\n",ch);
belsarekaiwalya 5:84e63108a1b6 194
belsarekaiwalya 5:84e63108a1b6 195 switch(ch)
belsarekaiwalya 5:84e63108a1b6 196 {
belsarekaiwalya 5:84e63108a1b6 197
belsarekaiwalya 5:84e63108a1b6 198 case 'w'://Front
belsarekaiwalya 5:84e63108a1b6 199 runMotor('L', 1, 0.7);
belsarekaiwalya 5:84e63108a1b6 200 runMotor('R', 1, 0.65);
belsarekaiwalya 5:84e63108a1b6 201 //serial.printf("A Forward\n");
belsarekaiwalya 5:84e63108a1b6 202 lcd.cls();
belsarekaiwalya 5:84e63108a1b6 203 lcd.locate(1,0);
belsarekaiwalya 5:84e63108a1b6 204 lcd.printf("Forward\n");
belsarekaiwalya 5:84e63108a1b6 205 break;
belsarekaiwalya 5:84e63108a1b6 206
belsarekaiwalya 5:84e63108a1b6 207 case 'd'://Right
belsarekaiwalya 5:84e63108a1b6 208 runMotor('L', 1, 1);
belsarekaiwalya 5:84e63108a1b6 209 runMotor('R', 0, 1);
belsarekaiwalya 5:84e63108a1b6 210 //serial.printf("A Right\n");
belsarekaiwalya 5:84e63108a1b6 211 lcd.cls();
belsarekaiwalya 5:84e63108a1b6 212 lcd.locate(1,0);
belsarekaiwalya 5:84e63108a1b6 213 lcd.printf("Right\n");
belsarekaiwalya 5:84e63108a1b6 214 break;
belsarekaiwalya 5:84e63108a1b6 215
belsarekaiwalya 5:84e63108a1b6 216 case 's'://Back
belsarekaiwalya 5:84e63108a1b6 217 runMotor('L', 0, 1);
belsarekaiwalya 5:84e63108a1b6 218 runMotor('R', 0, 1);
belsarekaiwalya 5:84e63108a1b6 219 //serial.printf("A Back\n");
belsarekaiwalya 5:84e63108a1b6 220 lcd.cls();
belsarekaiwalya 5:84e63108a1b6 221 lcd.locate(1,0);
belsarekaiwalya 5:84e63108a1b6 222 lcd.printf("Back\n");
belsarekaiwalya 5:84e63108a1b6 223 break;
belsarekaiwalya 5:84e63108a1b6 224
belsarekaiwalya 5:84e63108a1b6 225 case 'a'://Left
belsarekaiwalya 5:84e63108a1b6 226 runMotor('L', 0, 1);
belsarekaiwalya 5:84e63108a1b6 227 runMotor('R', 1, 1);
belsarekaiwalya 5:84e63108a1b6 228 //serial.printf("A Left\n");
belsarekaiwalya 5:84e63108a1b6 229 lcd.cls();
belsarekaiwalya 5:84e63108a1b6 230 lcd.locate(1,0);
belsarekaiwalya 5:84e63108a1b6 231 lcd.printf("Left\n");
belsarekaiwalya 5:84e63108a1b6 232 break;
belsarekaiwalya 5:84e63108a1b6 233
belsarekaiwalya 5:84e63108a1b6 234 case 'A'://Anticlock
belsarekaiwalya 5:84e63108a1b6 235 runMotor('L', 0, 0.45);
belsarekaiwalya 5:84e63108a1b6 236 runMotor('R', 1, 0.41);
belsarekaiwalya 5:84e63108a1b6 237 //serial.printf("A AntiClock\n");
belsarekaiwalya 5:84e63108a1b6 238 lcd.cls();
belsarekaiwalya 5:84e63108a1b6 239 lcd.locate(1,0);
belsarekaiwalya 5:84e63108a1b6 240 lcd.printf("AntiClock\n");
belsarekaiwalya 5:84e63108a1b6 241 break;
belsarekaiwalya 5:84e63108a1b6 242
belsarekaiwalya 5:84e63108a1b6 243 case 'C'://Clock
belsarekaiwalya 5:84e63108a1b6 244 runMotor('L', 1, 0.45);
belsarekaiwalya 5:84e63108a1b6 245 runMotor('R', 0, 0.41);
belsarekaiwalya 5:84e63108a1b6 246 //serial.printf("A Clock\n");
belsarekaiwalya 5:84e63108a1b6 247 lcd.cls();
belsarekaiwalya 5:84e63108a1b6 248 lcd.locate(1,0);
belsarekaiwalya 5:84e63108a1b6 249 lcd.printf("Clock\n");
belsarekaiwalya 5:84e63108a1b6 250 break;
belsarekaiwalya 5:84e63108a1b6 251
belsarekaiwalya 5:84e63108a1b6 252 case 'O'://Motor Stop
belsarekaiwalya 5:84e63108a1b6 253 Brake();
belsarekaiwalya 5:84e63108a1b6 254 //rolA = 0;
belsarekaiwalya 5:84e63108a1b6 255 //rolB = 0;
belsarekaiwalya 5:84e63108a1b6 256 //RolPwm = 0;
belsarekaiwalya 5:84e63108a1b6 257 if(jim1==0)
belsarekaiwalya 5:84e63108a1b6 258 {
belsarekaiwalya 5:84e63108a1b6 259 rolStop();
belsarekaiwalya 5:84e63108a1b6 260 jim1++;
belsarekaiwalya 5:84e63108a1b6 261 }
belsarekaiwalya 5:84e63108a1b6 262 jim1=0;
belsarekaiwalya 5:84e63108a1b6 263 //serial.printf("A Motor STOP\n");
belsarekaiwalya 5:84e63108a1b6 264 lcd.cls();
belsarekaiwalya 5:84e63108a1b6 265 lcd.locate(1,0);
belsarekaiwalya 5:84e63108a1b6 266 lcd.printf("STOP\n");
belsarekaiwalya 5:84e63108a1b6 267 break;
belsarekaiwalya 5:84e63108a1b6 268
belsarekaiwalya 5:84e63108a1b6 269 case 'b':
belsarekaiwalya 5:84e63108a1b6 270 if(jim==0)
belsarekaiwalya 5:84e63108a1b6 271 {
belsarekaiwalya 5:84e63108a1b6 272 runRol();
belsarekaiwalya 5:84e63108a1b6 273 jim++;
belsarekaiwalya 5:84e63108a1b6 274 }
belsarekaiwalya 3:86145fd89b45 275 break;
belsarekaiwalya 5:84e63108a1b6 276
belsarekaiwalya 5:84e63108a1b6 277 default:
belsarekaiwalya 5:84e63108a1b6 278 Brake();
belsarekaiwalya 5:84e63108a1b6 279 //serial.printf("Brake");
belsarekaiwalya 5:84e63108a1b6 280 lcd.cls();
belsarekaiwalya 5:84e63108a1b6 281 lcd.locate(1,0);
belsarekaiwalya 5:84e63108a1b6 282 lcd.printf("Brake");
belsarekaiwalya 5:84e63108a1b6 283 }
belsarekaiwalya 5:84e63108a1b6 284 jim=0;jim1=0;
belsarekaiwalya 5:84e63108a1b6 285 }
belsarekaiwalya 5:84e63108a1b6 286 }
belsarekaiwalya 5:84e63108a1b6 287 }
belsarekaiwalya 5:84e63108a1b6 288
belsarekaiwalya 5:84e63108a1b6 289 void runRol()
belsarekaiwalya 5:84e63108a1b6 290 {
belsarekaiwalya 5:84e63108a1b6 291 rolA = 1;
belsarekaiwalya 5:84e63108a1b6 292 rolB = 0;
belsarekaiwalya 5:84e63108a1b6 293 for (hoq = 0.0;hoq < 0.3; hoq = hoq + 0.01)
belsarekaiwalya 5:84e63108a1b6 294 {
belsarekaiwalya 5:84e63108a1b6 295 RolPwm = hoq;
belsarekaiwalya 5:84e63108a1b6 296 wait(0.05);
belsarekaiwalya 5:84e63108a1b6 297 }
belsarekaiwalya 5:84e63108a1b6 298 }
belsarekaiwalya 5:84e63108a1b6 299
belsarekaiwalya 5:84e63108a1b6 300 void rolStop()
belsarekaiwalya 5:84e63108a1b6 301 {
belsarekaiwalya 5:84e63108a1b6 302 rolA = 1;
belsarekaiwalya 5:84e63108a1b6 303 rolB = 0;
belsarekaiwalya 5:84e63108a1b6 304 for (hoq < 0.3;hoq > 0; hoq = hoq - 0.005)
belsarekaiwalya 5:84e63108a1b6 305 {
belsarekaiwalya 5:84e63108a1b6 306 RolPwm = hoq;
belsarekaiwalya 5:84e63108a1b6 307 wait(0.05);
belsarekaiwalya 5:84e63108a1b6 308 }
belsarekaiwalya 5:84e63108a1b6 309
belsarekaiwalya 5:84e63108a1b6 310 }
belsarekaiwalya 5:84e63108a1b6 311 void allfunction()
belsarekaiwalya 5:84e63108a1b6 312 {
belsarekaiwalya 5:84e63108a1b6 313 outputPID = updatePID();
belsarekaiwalya 5:84e63108a1b6 314 if(outputPID < 0)
belsarekaiwalya 5:84e63108a1b6 315 {runMotor('L', 1, (xpwmL - outputPID));
belsarekaiwalya 5:84e63108a1b6 316 runMotor('R', 1, (xpwmR + outputPID));}
belsarekaiwalya 5:84e63108a1b6 317 else if(outputPID > 0)
belsarekaiwalya 5:84e63108a1b6 318 {runMotor('L', 1, (xpwmL - outputPID));
belsarekaiwalya 5:84e63108a1b6 319 runMotor('R', 1, (xpwmR + outputPID));}
belsarekaiwalya 5:84e63108a1b6 320 else
belsarekaiwalya 5:84e63108a1b6 321 {runMotor('L', 1, (xpwmL));
belsarekaiwalya 5:84e63108a1b6 322 runMotor('R', 1, (xpwmR));}
belsarekaiwalya 5:84e63108a1b6 323
belsarekaiwalya 5:84e63108a1b6 324 //execPIDL();
belsarekaiwalya 5:84e63108a1b6 325 //execPIDR();
belsarekaiwalya 4:b084af72f9a6 326
belsarekaiwalya 5:84e63108a1b6 327 }
belsarekaiwalya 5:84e63108a1b6 328 float updatePID()
belsarekaiwalya 5:84e63108a1b6 329 {
belsarekaiwalya 5:84e63108a1b6 330 float lastError,pidTerm,P,D;
belsarekaiwalya 5:84e63108a1b6 331 err = CountDiffL - CountDiffR;
belsarekaiwalya 5:84e63108a1b6 332 P = err*Kp;
belsarekaiwalya 5:84e63108a1b6 333 D = (err - lastError)*Kd;
belsarekaiwalya 5:84e63108a1b6 334 pidTerm = (P + D);
belsarekaiwalya 5:84e63108a1b6 335 lastError = err;
belsarekaiwalya 5:84e63108a1b6 336 return pidTerm;
belsarekaiwalya 5:84e63108a1b6 337 }
belsarekaiwalya 5:84e63108a1b6 338 float updatePIDL()
belsarekaiwalya 5:84e63108a1b6 339 {
belsarekaiwalya 5:84e63108a1b6 340 float error,lastError,pidTerm,P,D;
belsarekaiwalya 5:84e63108a1b6 341 error = xpwmL - xpwmLc;
belsarekaiwalya 5:84e63108a1b6 342 P = error*KpL;
belsarekaiwalya 5:84e63108a1b6 343 D = (error - lastError)*KdL;
belsarekaiwalya 5:84e63108a1b6 344 pidTerm = (P + D);
belsarekaiwalya 5:84e63108a1b6 345 lastError = error;
belsarekaiwalya 5:84e63108a1b6 346 return pidTerm;
belsarekaiwalya 5:84e63108a1b6 347 }
belsarekaiwalya 5:84e63108a1b6 348 float updatePIDR()
belsarekaiwalya 5:84e63108a1b6 349 {
belsarekaiwalya 5:84e63108a1b6 350 float error,lastError,pidTerm,P,D;
belsarekaiwalya 5:84e63108a1b6 351 error = xpwmR - xpwmRc;
belsarekaiwalya 5:84e63108a1b6 352 P = error*KpR;
belsarekaiwalya 5:84e63108a1b6 353 D = (error - lastError)*KdR;
belsarekaiwalya 5:84e63108a1b6 354 pidTerm = (P + D);
belsarekaiwalya 5:84e63108a1b6 355 lastError = error;
belsarekaiwalya 5:84e63108a1b6 356 return pidTerm;
belsarekaiwalya 5:84e63108a1b6 357 }
belsarekaiwalya 5:84e63108a1b6 358 void keepEnCount()
belsarekaiwalya 5:84e63108a1b6 359 {
belsarekaiwalya 5:84e63108a1b6 360 Count1 = CountL;
belsarekaiwalya 5:84e63108a1b6 361 CountDiffL = Count1 - CountPrevL;
belsarekaiwalya 5:84e63108a1b6 362 CountPrevL = Count1 ;
belsarekaiwalya 5:84e63108a1b6 363
belsarekaiwalya 5:84e63108a1b6 364 Count2 = CountR;
belsarekaiwalya 5:84e63108a1b6 365 CountDiffR = Count2 - CountPrevR;
belsarekaiwalya 5:84e63108a1b6 366 CountPrevR = Count2 ;
belsarekaiwalya 5:84e63108a1b6 367 }
belsarekaiwalya 5:84e63108a1b6 368 void execPIDL()
belsarekaiwalya 5:84e63108a1b6 369 {
belsarekaiwalya 5:84e63108a1b6 370 if(correctPwmL < 0)
belsarekaiwalya 5:84e63108a1b6 371 {runMotor('L', 1, (xpwmL + correctPwmL));}
belsarekaiwalya 5:84e63108a1b6 372 else if(correctPwmL > 0)
belsarekaiwalya 5:84e63108a1b6 373 {runMotor('L', 1, (xpwmL + correctPwmL));}
belsarekaiwalya 5:84e63108a1b6 374 else
belsarekaiwalya 5:84e63108a1b6 375 {runMotor('L', 1, (xpwmL));}
belsarekaiwalya 5:84e63108a1b6 376 }
belsarekaiwalya 5:84e63108a1b6 377 void execPIDR()
belsarekaiwalya 5:84e63108a1b6 378 {
belsarekaiwalya 5:84e63108a1b6 379 if(correctPwmR < 0)
belsarekaiwalya 5:84e63108a1b6 380 {runMotor('R', 1, (xpwmR + correctPwmR));}
belsarekaiwalya 5:84e63108a1b6 381 else if(correctPwmR > 0)
belsarekaiwalya 5:84e63108a1b6 382 {runMotor('R', 1, (xpwmR + correctPwmR));}
belsarekaiwalya 5:84e63108a1b6 383 else
belsarekaiwalya 5:84e63108a1b6 384 {runMotor('R', 1, (xpwmR));}
belsarekaiwalya 5:84e63108a1b6 385 }
belsarekaiwalya 5:84e63108a1b6 386 void runMotor(char mot, bool dir, float pwm)
belsarekaiwalya 5:84e63108a1b6 387 {
belsarekaiwalya 5:84e63108a1b6 388 switch(mot) {
belsarekaiwalya 5:84e63108a1b6 389 case 'L':
belsarekaiwalya 5:84e63108a1b6 390 if(dir) {
belsarekaiwalya 5:84e63108a1b6 391 dirLa = 1;
belsarekaiwalya 5:84e63108a1b6 392 dirLb = 0;
belsarekaiwalya 5:84e63108a1b6 393 } else {
belsarekaiwalya 5:84e63108a1b6 394 dirLa = 0;
belsarekaiwalya 5:84e63108a1b6 395 dirLb = 1;
belsarekaiwalya 5:84e63108a1b6 396 }
belsarekaiwalya 5:84e63108a1b6 397 pwmL= pwm;
belsarekaiwalya 5:84e63108a1b6 398 //runSmoothL(pwm);
belsarekaiwalya 5:84e63108a1b6 399 break;
belsarekaiwalya 5:84e63108a1b6 400
belsarekaiwalya 5:84e63108a1b6 401 case 'R':
belsarekaiwalya 5:84e63108a1b6 402 if(dir) {
belsarekaiwalya 5:84e63108a1b6 403 dirRa = 1;
belsarekaiwalya 5:84e63108a1b6 404 dirRb = 0;
belsarekaiwalya 5:84e63108a1b6 405 } else {
belsarekaiwalya 5:84e63108a1b6 406 dirRa = 0;
belsarekaiwalya 5:84e63108a1b6 407 dirRb = 1;
belsarekaiwalya 5:84e63108a1b6 408 }
belsarekaiwalya 5:84e63108a1b6 409 pwmR= pwm;
belsarekaiwalya 5:84e63108a1b6 410 //runSmoothR(pwm);
belsarekaiwalya 5:84e63108a1b6 411 break;
belsarekaiwalya 5:84e63108a1b6 412
belsarekaiwalya 5:84e63108a1b6 413 default:
belsarekaiwalya 5:84e63108a1b6 414 Brake();
belsarekaiwalya 5:84e63108a1b6 415 }
belsarekaiwalya 5:84e63108a1b6 416 }
belsarekaiwalya 5:84e63108a1b6 417 void linDist(unsigned int DistanceInCM)
belsarekaiwalya 5:84e63108a1b6 418 {
belsarekaiwalya 5:84e63108a1b6 419 float ReqdShaftCount = 0;
belsarekaiwalya 5:84e63108a1b6 420 unsigned long int ReqdShaftCountInt = 0;
belsarekaiwalya 5:84e63108a1b6 421
belsarekaiwalya 5:84e63108a1b6 422 ReqdShaftCount = DistanceInCM*31.0;
belsarekaiwalya 5:84e63108a1b6 423 ReqdShaftCountInt = (unsigned long int) ReqdShaftCount;
belsarekaiwalya 5:84e63108a1b6 424 CountL = 0;
belsarekaiwalya 5:84e63108a1b6 425 CountR = 0;
belsarekaiwalya 5:84e63108a1b6 426
belsarekaiwalya 5:84e63108a1b6 427 while(1) {
belsarekaiwalya 5:84e63108a1b6 428 wait(0.01);
belsarekaiwalya 5:84e63108a1b6 429 if((CountL + CountR)*0.5 > ReqdShaftCountInt) {
belsarekaiwalya 5:84e63108a1b6 430 break;
belsarekaiwalya 0:c9c8c027609e 431 }
belsarekaiwalya 0:c9c8c027609e 432 }
belsarekaiwalya 5:84e63108a1b6 433 Brake();
belsarekaiwalya 0:c9c8c027609e 434 }
belsarekaiwalya 5:84e63108a1b6 435 void botRot(int Degrees)
belsarekaiwalya 4:b084af72f9a6 436 {
belsarekaiwalya 5:84e63108a1b6 437 float ReqdShaftCount = 0;
belsarekaiwalya 5:84e63108a1b6 438 unsigned long int ReqdShaftCountInt = 0;
belsarekaiwalya 5:84e63108a1b6 439
belsarekaiwalya 5:84e63108a1b6 440 ReqdShaftCount = Degrees* 5.6;
belsarekaiwalya 5:84e63108a1b6 441 ReqdShaftCountInt = (unsigned int) ReqdShaftCount;
belsarekaiwalya 5:84e63108a1b6 442 CountL = 0;
belsarekaiwalya 5:84e63108a1b6 443 CountR = 0;
belsarekaiwalya 5:84e63108a1b6 444 while (true) {
belsarekaiwalya 5:84e63108a1b6 445 wait(0.01);
belsarekaiwalya 5:84e63108a1b6 446 if((CountL >= ReqdShaftCountInt) | (CountR >= ReqdShaftCountInt)) {
belsarekaiwalya 5:84e63108a1b6 447 break;
belsarekaiwalya 4:b084af72f9a6 448 }
belsarekaiwalya 5:84e63108a1b6 449 }
belsarekaiwalya 5:84e63108a1b6 450 Brake();
belsarekaiwalya 0:c9c8c027609e 451 }
belsarekaiwalya 3:86145fd89b45 452 void Brake()
belsarekaiwalya 0:c9c8c027609e 453 {
belsarekaiwalya 1:59c416ccba42 454 dirLa = 0;
belsarekaiwalya 1:59c416ccba42 455 dirLb = 0;
belsarekaiwalya 1:59c416ccba42 456 pwmL=0.0;
belsarekaiwalya 5:84e63108a1b6 457
belsarekaiwalya 4:b084af72f9a6 458 dirRa = 0;
belsarekaiwalya 4:b084af72f9a6 459 dirRb = 0;
belsarekaiwalya 5:84e63108a1b6 460 pwmR=0.0;
belsarekaiwalya 0:c9c8c027609e 461 }