Diff: main.cpp

Revision:

1:84167ca00307

Parent:

0:451c27e4d55e

Child:

6:98871feebea0

--- a/main.cpp	Mon Dec 07 14:41:42 2015 +0000
+++ b/main.cpp	Thu Jan 14 18:31:28 2016 +0000
@@ -5,48 +5,55 @@
 #include "PID.h"
 #include "Motor.h"
 #include "eeprom.h"
+#include "Reciever.h" 
 
 //*****************************************************/
-// Defines  // 
-#define Rate 0.01
-#define Kc  -2.6
-#define Ti   0.0
-#define Td   0.0
+//--PID parameter--
+//-Upper-//
+int U_Kc;
+int U_Ti;
+int U_Td;
+//-lower-//
+int L_Kc;
+int L_Ti;
+int L_Td;
 
 //*****************************************************/
 // Global  //
-//-- pc monitor --
+//-- Communication --
 Serial PC(D1,D0);
+ANDANTE_PROTOCOL_PACKET command;
 //-- encoder --  
-int Position;
+int Upper_Position;
+int Lower_Position;
 int data;
-SPI device(Emosi, Emiso, Esck);
-DigitalOut Encoder(EncoderA);
+SPI ENC(Emosi, Emiso, Esck);
+DigitalOut EncA(EncoderA);
+DigitalOut EncB(EncoderB);
 //-- Motor --
-Motor LeftUpper(PWM_LU,A_LU,B_LU);
+int dir;
+Motor Upper(PWM_LU,A_LU,B_LU);
+Motor Lower(PWM_LL,A_LL,B_LL);
 //-- PID --
-int SetPoint;
-PID LU_PID(Kc, Ti, Td, Rate);//Kp,Ki,Kd,Rate
+int Upper_SetPoint;
+int Lower_SetPoint;
+PID Up_PID(U_Kc, U_Ti, U_Td, 0.001);//Kp,Ki,Kd,Rate
+PID Low_PID(L_Kc, L_Ti, L_Td, 0.001);
 //*****************************************************/
-void Read_Encoder()
+void Read_Encoder(PinName Encoder)
 {
-    SPI device(Emosi, Emiso, Esck);
-    device.format(8,0);
-    device.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k
+    Enc.format(8,0);
+    Enc.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k
     
     Encoder = 0;
-    wait_us(50);
-    device.write(0x41);
-    wait_us(50);
-    device.write(0x09);
-    wait_us(50);
-    data = device.write(0x00);
-    wait_us(50);
+    Enc.write(0x41);
+    Enc.write(0x09);
+    data = Enc.write(0x00);
     Encoder = 1;
     
 }
 //*****************************************************/
-void Get_EnValue(int Val)
+int Get_EnValue(int Val)
 {
     int i = 0;
     static unsigned char codes[] = {
@@ -59,43 +66,79 @@
     253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190, 
     254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95 };
     
-    while(i<=127)
+    while(Val != codes[i])
     {
-        if(Val == codes[i])
-        {
-            Position = i;
-            break;
-        }
-        else i++;
+        i++;
     }
+    
+    return i;
+    
 }
 //*****************************************************/
+void SET_UpperPID()
+{
+    Upper.period(0.001);
+    Up_PID.setMode(0);
+    Up_PID.setInputLimits(0,127);
+    Up_PID.setOutputLimits(0,1);
+}
+//******************************************************/
+void SET_LowerPID()
+{
+    Lower.period(0.001);
+    Low_PID.setMode(0);
+    Low_PID.setInputLimits(0,127);
+    Low_PID.setOutputLimits(0,1);
+}
+//******************************************************/   
+void Move_Upper()
+{
+    Read_Encoder(EncoderA);
+    Upper_Position = Get_EnValue(data);
+
+    Up_PID.setProcessValue(Upper_Position);
+            
+    if(Upper_Position - Uppper_SetPoint > 0 ){ 
+        dir = 1;
+        }
+    if(Upper_Position - Uppper_SetPoint < 0 ){ 
+        dir = -1;
+        }
+    Upper.speed(LU_PID.compute() * dir);
+}
+//******************************************************/
+void Move_Lower()
+{
+    Read_Encoder(EncoderB);
+    Lower_Position = Get_EnValue(data);
+
+    Low_PID.setProcessValue(Lower_Position);
+            
+    if(Lower_Position - Lower_SetPoint > 0 ){ 
+        dir = 1;
+        }
+    if(Lower_Position - Lower_SetPoint < 0 ){ 
+        dir = -1;
+        }
+    Lower.speed(LU_PID.compute() * dir);
+}   
+//******************************************************/
+
+
 int main()
 {
-    LeftUpper.period(0.00005);
-    LU_PID.setInputLimits(0,127);
-    LU_PID.setOutputLimits(0,0.9);
-    LU_PID.setMode(AUTO_MODE);
-    
-    //get the target position
-    SetPoint = 63;
-    LU_PID.setSetPoint(SetPoint);
+    SET_UpperPID();
+    SET_LowerPID();
     
-    Read_Encoder();
-    PC.printf("%d\n",data);
-    Get_EnValue(data);
-    PC.printf("%d\n********************\n",Position);    
-    
-    while(Position != SetPoint)
+    while(1) 
     {
-        LU_PID.setProcessValue(Position);
-        LeftUpper.speed(LU_PID.compute());
-        
-        Read_Encoder();
-        Get_EnValue(data);
+        Up_PID.setSetPoint(Upper_SetPoint);
+        Low_PID.setSetPoint(Lower_SetPoint);
+
+        Move_Upper();
+        Move_Lower();
     }
-    
-       
+
 }