Revision 5:84e63108a1b6, committed 2020-03-11

Comitter:

belsarekaiwalya

Date:

Wed Mar 11 10:02:43 2020 +0000

Parent:

4:b084af72f9a6

Commit message:

h

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.lib	Wed Mar 11 10:02:43 2020 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/garfieldsg/code/MPU6050/#1e0baaf91e96

--- a/QEI.lib	Wed Mar 22 07:15:20 2017 +0000
+++ b/QEI.lib	Wed Mar 11 10:02:43 2020 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
+http://mbed.org/users/electromotivated/code/QEI/#50aae578cb89

--- 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