![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Code
Dependencies: mbed QEI MPU6050 TextLCD
Diff: main.cpp
- Revision:
- 5:84e63108a1b6
- Parent:
- 4:b084af72f9a6
diff -r b084af72f9a6 -r 84e63108a1b6 main.cpp --- a/main.cpp Wed Mar 22 07:15:20 2017 +0000 +++ b/main.cpp Wed Mar 11 10:02:43 2020 +0000 @@ -1,27 +1,63 @@ #include "mbed.h" #include "TextLCD.h" +#include "QEI.h" + +#define ppr 1120 +#define diam 11.0 +#define circum (3.14*diam) +#define width 23.0 +#define motRPM 150 + TextLCD lcd(PA_0,PA_1,PA_4,PB_0,PC_1,PC_0); // rs, e, d4-d7 +QEI leftEnc(PB_8,PB_9); +QEI rightEnc(PA_6,PA_5); + Serial serial(USBTX, USBRX); -Serial bt(PC_6,PC_7);//tx,rx -DigitalOut dirRa(PB_3); -DigitalOut dirRb(PB_5); +//Serial hmcRead(PC_10,PC_11);//tx rx +Serial bt(PA_9,PA_10);//tx,rx +Serial xbee1(PC_10,PC_11); + DigitalOut dirLa(PB_4); DigitalOut dirLb(PB_10); -PwmOut pwmL(PA_8);//left -PwmOut pwmR(PA_9);//right +PwmOut pwmR(PA_8);//left +DigitalOut dirRa(PB_5); +DigitalOut dirRb(PB_3); +PwmOut pwmL(PC_7);//right +Ticker TISR; +Ticker Print; + +DigitalOut rolA(PA_13); +DigitalOut rolB(PA_14); +PwmOut RolPwm(PA_15);//roller + +void rolStop(); void InitSerial(); void runMotor(char mot, bool dir, float pwm); -void front(float xpwmL,float xpwmR); -void back(float xpwm,float xpwmR); -void left(float xpwm, float xpwmR); -void right(float xpwm,float xpwmR); -void clockWise(float xpwm,float xpwmR); -void antiClock(float xpwm,float xpwmR); +float updatePIDL(); +float updatePIDR(); +void keepEnCount(void); +void printData(void); +void execPIDL(); +void execPIDR(); +void hmcCorrect(); +float updatePID();double hoq; +void allfunction(); +void runRol(); void Brake(); - +float err; int ch; - +char run_mode; +bool zom = true;int jim=0,jim1=0; +long Count1,Count2; +long CountL,CountR,CountDiffL,CountDiffR,CountPrevL,CountPrevR; +float correctPwmL,correctPwmR; +double distL,distR,distAv,velL,velR; +float xpwmL=0.8*1.1,xpwmR=0.8; +float xpwmLc,xpwmRc,xpwmLR,outputPID; +double KpL=0.1,KpR=0.125,KdL=0,KdR=0,Kp=0.3,Kd = 0; +//int Mx,Mx1,err,lastError; +//float outputPID,P,D; void InitSerial() { bt.baud(9600); @@ -29,199 +65,397 @@ } void SetPwmf_kHz(float freq) { - - pwmL.period_ms(freq); - pwmR.period_ms(freq); - -} + + pwmL.period_ms(1/freq); + pwmR.period_ms(1/freq); + RolPwm.period_ms(1/freq); +} +/*void hmcCorrect() +{ + err = Mx1 - Mx; + P = err*KpL; + D = (err - lastError)*KdL; + outputPID = (P + D); + lastError = err; + if(outputPID < 0) + {runMotor('L', 1, (xpwmL + outputPID)); + runMotor('R', 1, (xpwmR - outputPID));} + else if(outputPID > 0) + {runMotor('L', 1, (xpwmL + outputPID)); + runMotor('R', 1, (xpwmR - outputPID));} + else + {runMotor('L', 1, (xpwmL)); + runMotor('R', 1, (xpwmR));} + }*/ +void printData() +{ + // lcd.cls(); + // lcd.locate(1,0); + //lcd.printf("%f",outputPID ); + //lcd.locate(1,1); + //lcd.printf("%f",velR); +} int main() { InitSerial(); SetPwmf_kHz(1); + TISR.attach(&keepEnCount, 0.01); + Print.attach(&printData, 0.05); + lcd.cls(); Brake(); + while(1) { - if(bt.readable()) + + //rolA=1;rolB=0; +/* CountL = leftEnc.read(); + CountR = rightEnc.read(); + distL = (CountL/ppr)*circum; + distR = (CountR/ppr)*circum; + distAv = (0.5*(distL + distR)); + xpwmLc = float(CountDiffL*60)/(ppr*motRPM); + xpwmRc = float(CountDiffR*60)/(ppr*motRPM); + xpwmLR = float((CountDiffL-CountDiffR)*60)/(ppr*motRPM); + velL = (circum*motRPM*xpwmLc)/60; + velR = (circum*motRPM*xpwmRc)/60; + correctPwmL = updatePIDL(); + correctPwmR = updatePIDR(); + outputPID = updatePID(); + if(outputPID < 0) + {runMotor('L', 1, (xpwmL - outputPID)); + runMotor('R', 1, (xpwmR + outputPID));} + else if(outputPID > 0) + {runMotor('L', 1, (xpwmL - outputPID)); + runMotor('R', 1, (xpwmR + outputPID));} + else + {runMotor('L', 1, (xpwmL)); + runMotor('R', 1, (xpwmR));} + + //execPIDL(); + //execPIDR(); + + + */ + CountL = leftEnc.read(); + CountR = rightEnc.read(); + distL = (CountL/ppr)*circum; + distR = (CountR/ppr)*circum; + distAv = (0.5*(distL + distR)); + xpwmLc = float(CountDiffL*60)/(ppr*motRPM); + xpwmRc = float(CountDiffR*60)/(ppr*motRPM); + xpwmLR = float((CountDiffL-CountDiffR)*60)/(ppr*motRPM); + velL = (circum*motRPM*xpwmLc)/60; + velR = (circum*motRPM*xpwmRc)/60; + correctPwmL = updatePIDL(); + correctPwmR = updatePIDR(); + //RolPwm = 0.5; + + if(xbee1.readable()) { - ch = bt.getc(); - lcd.cls(); - switch(ch) + run_mode = xbee1.getc(); + //serial.printf("%c\n",run_mode); + switch(run_mode) { - case 'w'://Front - runMotor('L', 1, 0.5); - runMotor('R', 1, 0.5); - serial.printf("Forward\n"); - lcd.cls(); - lcd.locate(1,0); - lcd.printf("Forward\n"); + case 'd': + runMotor('L', 1, 0.7); + runMotor('R', 1, 0.65); + serial.printf("M Forward\n"); + break;//front + + case 'a': + runMotor('L', 1, 1); + runMotor('R', 0, 1); + serial.printf("M Right\n"); + break;//right + + case 's': + runMotor('L', 0, 1); + runMotor('R', 0, 1); + serial.printf("M Back\n"); + break;//back + + case 'w': + runMotor('L', 0, 1); + runMotor('R', 1, 1); + serial.printf("M Left\n"); + break;//left + + case 'x': + runMotor('L', 0, 0); + runMotor('R', 1, 0); + serial.printf("M Stop\n"); + break;//stop + } + } + else if(bt.readable()) + { + ch = bt.getc(); + serial.printf("%c\n",ch); + + switch(ch) + { + + case 'w'://Front + runMotor('L', 1, 0.7); + runMotor('R', 1, 0.65); + //serial.printf("A Forward\n"); + lcd.cls(); + lcd.locate(1,0); + lcd.printf("Forward\n"); + break; + + case 'd'://Right + runMotor('L', 1, 1); + runMotor('R', 0, 1); + //serial.printf("A Right\n"); + lcd.cls(); + lcd.locate(1,0); + lcd.printf("Right\n"); + break; + + case 's'://Back + runMotor('L', 0, 1); + runMotor('R', 0, 1); + //serial.printf("A Back\n"); + lcd.cls(); + lcd.locate(1,0); + lcd.printf("Back\n"); + break; + + case 'a'://Left + runMotor('L', 0, 1); + runMotor('R', 1, 1); + //serial.printf("A Left\n"); + lcd.cls(); + lcd.locate(1,0); + lcd.printf("Left\n"); + break; + + case 'A'://Anticlock + runMotor('L', 0, 0.45); + runMotor('R', 1, 0.41); + //serial.printf("A AntiClock\n"); + lcd.cls(); + lcd.locate(1,0); + lcd.printf("AntiClock\n"); + break; + + case 'C'://Clock + runMotor('L', 1, 0.45); + runMotor('R', 0, 0.41); + //serial.printf("A Clock\n"); + lcd.cls(); + lcd.locate(1,0); + lcd.printf("Clock\n"); + break; + + case 'O'://Motor Stop + Brake(); + //rolA = 0; + //rolB = 0; + //RolPwm = 0; + if(jim1==0) + { + rolStop(); + jim1++; + } + jim1=0; + //serial.printf("A Motor STOP\n"); + lcd.cls(); + lcd.locate(1,0); + lcd.printf("STOP\n"); + break; + + case 'b': + if(jim==0) + { + runRol(); + jim++; + } break; - - case 'a'://Left - runMotor('L', 1, 0); - runMotor('R', 1, 0.5); - serial.printf("Left\n"); - lcd.cls(); - lcd.locate(1,0); - lcd.printf("Left\n"); - break; - - case 's'://Back - runMotor('L', 0, 0.5); - runMotor('R', 0, 0.5); - serial.printf("Back\n"); - lcd.cls(); - lcd.locate(1,0); - lcd.printf("Back\n"); - break; - - case 'd'://Right - runMotor('L', 1, 0.5); - runMotor('R', 1, 0); - serial.printf("Right\n"); - lcd.cls(); - lcd.locate(1,0); - lcd.printf("Right\n"); - break; - - case 'A'://Anticlock - runMotor('L', 0, 0.5); - runMotor('R', 1, 0.5); - serial.printf("AntiClock\n"); - lcd.cls(); - lcd.locate(1,0); - lcd.printf("AntiClock\n"); - break; - - case 'C'://Clock - runMotor('L', 1, 0.5); - runMotor('R', 0, 0.5); - serial.printf("Clock\n"); - lcd.cls(); - lcd.locate(1,0); - lcd.printf("Clock\n"); - break; + + default: + Brake(); + //serial.printf("Brake"); + lcd.cls(); + lcd.locate(1,0); + lcd.printf("Brake"); + } + jim=0;jim1=0; + } + } +} + +void runRol() +{ +rolA = 1; +rolB = 0; +for (hoq = 0.0;hoq < 0.3; hoq = hoq + 0.01) +{ + RolPwm = hoq; + wait(0.05); +} +} + +void rolStop() +{ +rolA = 1; +rolB = 0; +for (hoq < 0.3;hoq > 0; hoq = hoq - 0.005) +{ + RolPwm = hoq; + wait(0.05); +} + +} +void allfunction() +{ + outputPID = updatePID(); + if(outputPID < 0) + {runMotor('L', 1, (xpwmL - outputPID)); + runMotor('R', 1, (xpwmR + outputPID));} + else if(outputPID > 0) + {runMotor('L', 1, (xpwmL - outputPID)); + runMotor('R', 1, (xpwmR + outputPID));} + else + {runMotor('L', 1, (xpwmL)); + runMotor('R', 1, (xpwmR));} + + //execPIDL(); + //execPIDR(); - case 'O'://Stop - Brake(); - serial.printf("STOP\n"); - lcd.cls(); - lcd.locate(1,0); - lcd.printf("STOP\n"); - break; - - default: - Brake(); - serial.printf("Brake"); - lcd.cls(); - lcd.locate(1,0); - lcd.printf("Brake"); - } + } +float updatePID() +{ + float lastError,pidTerm,P,D; + err = CountDiffL - CountDiffR; + P = err*Kp; + D = (err - lastError)*Kd; + pidTerm = (P + D); + lastError = err; + return pidTerm; +} +float updatePIDL() +{ + float error,lastError,pidTerm,P,D; + error = xpwmL - xpwmLc; + P = error*KpL; + D = (error - lastError)*KdL; + pidTerm = (P + D); + lastError = error; + return pidTerm; +} +float updatePIDR() +{ + float error,lastError,pidTerm,P,D; + error = xpwmR - xpwmRc; + P = error*KpR; + D = (error - lastError)*KdR; + pidTerm = (P + D); + lastError = error; + return pidTerm; +} +void keepEnCount() +{ + Count1 = CountL; + CountDiffL = Count1 - CountPrevL; + CountPrevL = Count1 ; + + Count2 = CountR; + CountDiffR = Count2 - CountPrevR; + CountPrevR = Count2 ; +} +void execPIDL() +{ + if(correctPwmL < 0) + {runMotor('L', 1, (xpwmL + correctPwmL));} + else if(correctPwmL > 0) + {runMotor('L', 1, (xpwmL + correctPwmL));} + else + {runMotor('L', 1, (xpwmL));} +} +void execPIDR() +{ + if(correctPwmR < 0) + {runMotor('R', 1, (xpwmR + correctPwmR));} + else if(correctPwmR > 0) + {runMotor('R', 1, (xpwmR + correctPwmR));} + else + {runMotor('R', 1, (xpwmR));} +} +void runMotor(char mot, bool dir, float pwm) +{ + switch(mot) { + case 'L': + if(dir) { + dirLa = 1; + dirLb = 0; + } else { + dirLa = 0; + dirLb = 1; + } + pwmL= pwm; + //runSmoothL(pwm); + break; + + case 'R': + if(dir) { + dirRa = 1; + dirRb = 0; + } else { + dirRa = 0; + dirRb = 1; + } + pwmR= pwm; + //runSmoothR(pwm); + break; + + default: + Brake(); + } +} +void linDist(unsigned int DistanceInCM) +{ + float ReqdShaftCount = 0; + unsigned long int ReqdShaftCountInt = 0; + + ReqdShaftCount = DistanceInCM*31.0; + ReqdShaftCountInt = (unsigned long int) ReqdShaftCount; + CountL = 0; + CountR = 0; + + while(1) { + wait(0.01); + if((CountL + CountR)*0.5 > ReqdShaftCountInt) { + break; } } + Brake(); } -void runMotor(char mot, bool dir, float pwm) +void botRot(int Degrees) { - switch(mot) - { - case 'L': - if(dir) - { - dirLa = 1; - dirLb = 0; - } - else - { - dirLa = 0; - dirLb = 1; - } - pwmL= pwm; - break; - - case 'R': - if(dir) - { - dirRa = 1; - dirRb = 0; - } - else - { - dirRa = 0; - dirRb = 1; + float ReqdShaftCount = 0; + unsigned long int ReqdShaftCountInt = 0; + + ReqdShaftCount = Degrees* 5.6; + ReqdShaftCountInt = (unsigned int) ReqdShaftCount; + CountL = 0; + CountR = 0; + while (true) { + wait(0.01); + if((CountL >= ReqdShaftCountInt) | (CountR >= ReqdShaftCountInt)) { + break; } - pwmR= pwm; - break; - - default: - Brake(); - } -} -void front(float xpwmL,float xpwmR) -{ - dirLa = 1; - dirLb = 0; - pwmL= xpwmL; - - dirRa = 1; - dirRb = 0; - pwmR= xpwmR; -} -void back(float xpwmL,float xpwmR) -{ - dirLa = 0; - dirLb = 1; - pwmL= xpwmL; - - dirRa = 0; - dirRb = 1; - pwmR= xpwmR; -} -void left(float xpwmL,float xpwmR) -{ - dirLa = 0; - dirLb = 0; - pwmL= xpwmL; - - dirRa = 1; - dirRb = 0; - pwmR= xpwmR; -} -void right(float xpwmL,float xpwmR) -{ - dirLa = 1; - dirLb = 0; - pwmL = xpwmL; - - dirRa = 0; - dirRb = 0; - pwmR = xpwmR; + } + Brake(); } void Brake() { dirLa = 0; dirLb = 0; pwmL=0.0; - + dirRa = 0; dirRb = 0; - pwmR=0.0; + pwmR=0.0; } -void antiClock(float xpwmL,float xpwmR) -{ - dirLa = 0; - dirLb = 1; - pwmL = xpwmL; - - dirRa = 1; - dirRb = 0; - pwmR = xpwmR; -} -void clockWise(float xpwmL,float xpwmR) -{ - dirLa = 1; - dirLb = 0; - pwmL = xpwmL; - - dirRa = 0; - dirRb = 1; - pwmR = xpwmR; -} \ No newline at end of file