Revision 1:f1d9b55f7dd7, committed 2016-03-05

Comitter:

ibestwill

Date:

Sat Mar 05 02:44:45 2016 +0000

Parent:

0:867c4eabf128

Commit message:

was

Changed in this revision

--- a/main.cpp	Fri Feb 26 21:28:57 2016 +0000
+++ b/main.cpp	Sat Mar 05 02:44:45 2016 +0000
@@ -23,7 +23,7 @@
 void Initialize(void);
 char DisplayMenu(void);
 void ManualControl(void);
-void BluetoothTest(void);
+void BluetoothModule(void);
 void PiControllerISR(void);
 void WdtFaultISR(void);
 void ExtCollisionISR(void);
@@ -37,7 +37,6 @@
 int SatValue(int x, int Limit);
 int Signextend(int x);
 void Userinterface(void);
-void QeiInterface(void);
 void UsRangefinder(void);
 void RcServoMotor(void);
 
@@ -51,16 +50,25 @@
 
 
 // Global variables for interrupt handler
+// Global variables for interrupt handler
 int32_t dPos_R, dTime_R, Vel_R;
-int32_t Setspeed_R;
+int32_t dPos_L, dTime_L, Vel_L;
+int32_t Setspeed_R=0;
+int32_t Setspeed_L=0;
 int32_t u_R=0,uS_R=0;
+int32_t u_L=0,uS_L=0;
 int32_t uP_R,uI_R;
-int32_t e_R;
+int32_t uP_L,uI_L;
+int32_t e_R,e_L;
 int32_t x_R=0, xT_R=0;
+int32_t x_L=0, xT_L=0;
 
 float T = 5000;  //us
 float y_R=0;
-float Kp_R=3.5, Ki_R=0.8;  
+float y_L=0;
+float Kp_R=2.5, Ki_R=0.01;  
+float Kp_L=2.5, Ki_L=0.01;  
+
 
 
 //**************************************************************************
@@ -70,17 +78,65 @@
 int main()
 {
 
-    //Initialize();
+
+    Initialize();
 
     //Userinterface();
     
     //UsRangefinder();
     
-    RcServoMotor();
+    //RcServoMotor();
+    
+ 
+
+    PeriodicInt.attach(&PiControllerISR, 0.0005);
+    
+    
+        do {
+        
+        //BluetoothModule();
+
+        Userinterface();
+
+        pc.printf("\r\nSR:%d       Vel_R:%d       ER:%d        XR:%d       uR:%d \r\n" ,Setspeed_R,Vel_R,e_R,x_R,u_R);
+        
+        pc.printf("\r\nSL:%d       Vel_L:%d       EL:%d        XL:%d       uL:%d \r\n\n\n" ,Setspeed_R,Vel_R,e_R,x_R,u_R);
+       
+       
+        wait(0.5); // Go to sleep for 500 ms
+
+    } while(true);
+    
+    
 
 }
 
 
+
+
+//------------------------------------------------------------
+void BluetoothModule(void) {
+     // Echo characters at serial Bluetooth Channel (p9, p10)to and from the pc.serial.port
+     // Connect TxD from Bluetooth to rx (p10); RxD from Bluetooth to tx (p9)
+     char x;
+     BluetoothSerial.baud(9600);
+     printf("\r\nBluetooth serial channel.\r\n");
+     x=0;
+     
+     if (pc.readable()){
+         x=pc.getc();
+         BluetoothSerial.putc(x); // Receive keyboard entry and send to bluetooth
+         pc.putc(x); //Echo character from MBED COMM
+     }
+     if (BluetoothSerial.readable()){
+         x=BluetoothSerial.getc(); // Receive character from bluetooth module
+         pc.putc(x); // Write character from Bluetooth module to terminal emulator on PC
+         BluetoothSerial.putc(x); // Echo character character from bluetooth COMM
+     }
+
+ }
+ 
+
 void UsRangefinder(void)
 {
     unsigned int V, duration;
@@ -166,22 +222,90 @@
 }
 
 
+
 //**************************************************************************
-void Userinterface(void)
-{
+void Userinterface(void){
+
+    
+    char dirkey;
+    
+    pc.printf("\n\rEnter -W- -A- -S- -D- for Manual Control:\n ");
+    pc.scanf("%c",&dirkey);
+    pc.printf("\n\rKey Entered:%c\n\n\r",dirkey);
+    
+    
+    //Forward
+    if(dirkey=='w'||dirkey=='W') {
+
+        if(Setspeed_R<35) {
+            Setspeed_R=Setspeed_R+5;
+        } else Setspeed_R=35;
 
-    pc.printf("\n\rEnter number:\n ");
-    pc.scanf("%d",&Setspeed_R);
-    pc.printf("\n\rYou Entered :%d\n\n",Setspeed_R);
+        if(Setspeed_L>-35) {
+            Setspeed_L=Setspeed_L-5;
+        } else Setspeed_L=-35;
+    }
+    
+    
+
+    //Backward
+    else if (dirkey=='s'||dirkey=='S') {
+
+        if(Setspeed_R>-35) {
+            Setspeed_R=Setspeed_R-5;
+        } else Setspeed_R=-35;
+
+        if(Setspeed_L<35) {
+            Setspeed_L=Setspeed_L+5;
+        } else Setspeed_L=35;
+    }
+    
+    //Left Turn
+    else if (dirkey=='a'||dirkey=='A') {
 
-    /*
-    DirR=1;
-    y_R=Setspeed_R/36*T;
-    pc.printf("y_R = %f\n",y_R);
-    PwmR.pulsewidth_us(y_R);
-    */
+        if(Setspeed_R<35) {
+            Setspeed_R=Setspeed_R+2;
+        } else Setspeed_R=35;
+
+        if(Setspeed_L>-35) {
+            Setspeed_L=Setspeed_L-1;
+        } else Setspeed_L=-35;
+        
+    }
+    
+    //Right Turn
+    else if (dirkey=='d'||dirkey=='D') {
+        
+        if(Setspeed_R<35) {
+            Setspeed_R=Setspeed_R+1;
+        } else Setspeed_R=35;
+
+        if(Setspeed_L>-35) {
+            Setspeed_L=Setspeed_L-2;
+        } else Setspeed_L=-35;    
 
-}
+    }
+    
+    //Stop
+    else if (dirkey=='x'||dirkey=='X') {
+     
+        Setspeed_R=0;
+        y_R=0;
+        
+        Setspeed_L=0;
+        y_L=0;
+        
+    }
+    
+    //Wrong Key Entered
+    else pc.printf("\n\rInvalid Key!\n\r "); 
+
+    
+
+}    
+
+
+
 
 //**************************************************************************
 void Initialize(void)
@@ -205,7 +329,7 @@
     SpiReset=1;
     wait_us(10);
     SpiReset=0;
-    int ID = DE0.write(0x8002);  //SPI slave control word to read (only) 2-word transactions starting at base address 0 within the peripheral
+    int ID = DE0.write(0x8004);  //SPI slave control word to read (only) 2-word transactions starting at base address 0 within the peripheral
 
     if(ID == 23) printf("\r\n\nSPI Interface is Successful !  ID = %d\r\n\n",ID); //ID=0x0017
     else printf("\r\n********SPI Interface failed!********\r\n\n");
@@ -214,22 +338,6 @@
 }
 
 
-//**************************************************************************
-void QeiInterface(void)
-{
-
-    dPos_R = DE0.write(0x00);
-    dTime_R = DE0.write(0x00);
-
-    dPos_R = Signextend(dPos_R);
-   
-
-    Vel_R = (123*dPos_R)/dTime_R;  //radians per second
-
-    printf("\r\ndPos_R: %d dTime_R: %d Vel_R: %d \r\n",dPos_R,dTime_R,Vel_R);
-
-
-}
 
 //**************************************************************************
 
@@ -237,43 +345,67 @@
 
     dPos_R = DE0.write(0x00);
     dTime_R = DE0.write(0x00);
+    
+    dPos_L = DE0.write(0x00);
+    dTime_L = DE0.write(0x00);
 
     dPos_R = Signextend(dPos_R);
-    
+    dPos_L = Signextend(dPos_L);
+   
 
-    Vel_R = (123*dPos_R)/dTime_R; 
+    Vel_R = (123*dPos_R)/dTime_R;  //radians per second
+    Vel_L = (123*dPos_L)/dTime_L;  //radians per second
     
 
     //Error
     e_R = SatSub(Setspeed_R,Vel_R);
-    //e_R = SatValue(e_R,36);
+    e_R = SatValue(e_R,70);
+    
+    e_L = SatSub(Setspeed_L,Vel_L);
+    e_L = SatValue(e_L,70);
+    
 
 
     //xtate
     xT_R = SatAdd(x_R,e_R);
-    //xT_R = SatValue(x_R,xlimit);
+    //xT_R = SatValue(x_R,320000);
 
+    xT_L = SatAdd(x_L,e_L);
+    //xT_L = SatValue(x_L,320000);
 
     //proportional and integral control terms
     uP_R=(Kp_R*e_R);
     uI_R=(Ki_R*x_R);
     
+    uP_L=(Kp_L*e_L);
+    uI_L=(Ki_L*x_L);
+    
     uS_R = SatAdd(uP_R,uI_R);
-    u_R = SatValue(uS_R,36);
+    u_R = SatValue(uS_R,35);
+     
+    uS_L = SatAdd(uP_L,uI_L);
+    u_L = SatValue(uS_L,35);
 
     if (u_R==uS_R) x_R = xT_R;  //Integrator windup prevention
+    if (u_L==uS_L) x_L = xT_L;  //Integrator windup prevention
+
 
     if(u_R >= 0) DirR = 1; else DirR = 0;
+    if(u_L >= 0) DirL = 0; else DirL = 1;
     
-    y_R=u_R/36.0*T;
+    y_R=u_R/35.0*T;
     PwmR.pulsewidth_us(abs(y_R));
     
+    y_L=u_L/35.0*T;
+    PwmL.pulsewidth_us(abs(y_L));
+    
 
 }
 
 
 //**************************************************************************
 
+
 int SatAdd(int x, int y)
 {
     // Detect overflow and saturate result
@@ -312,6 +444,7 @@
     return x;
 }
 
+
 //**************************************************************************
 //**************************************************************************