Diff: main.cpp

Revision:

0:451c27e4d55e

Child:

1:9f279c68ed0c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Dec 07 14:41:42 2015 +0000
@@ -0,0 +1,105 @@
+//*****************************************************/
+//  Include  // 
+#include "mbed.h"
+#include "pinconfig.h"
+#include "PID.h"
+#include "Motor.h"
+#include "eeprom.h"
+
+//*****************************************************/
+// Defines  // 
+#define Rate 0.01
+#define Kc  -2.6
+#define Ti   0.0
+#define Td   0.0
+
+//*****************************************************/
+// Global  //
+//-- pc monitor --
+Serial PC(D1,D0);
+//-- encoder --  
+int Position;
+int data;
+SPI device(Emosi, Emiso, Esck);
+DigitalOut Encoder(EncoderA);
+//-- Motor --
+Motor LeftUpper(PWM_LU,A_LU,B_LU);
+//-- PID --
+int SetPoint;
+PID LU_PID(Kc, Ti, Td, Rate);//Kp,Ki,Kd,Rate
+//*****************************************************/
+void Read_Encoder()
+{
+    SPI device(Emosi, Emiso, Esck);
+    device.format(8,0);
+    device.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);
+    Encoder = 1;
+    
+}
+//*****************************************************/
+void Get_EnValue(int Val)
+{
+    int i = 0;
+    static unsigned char codes[] = {
+    127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175,
+    191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215,
+    223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235, 
+    239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245, 
+    247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250,
+    251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125, 
+    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)
+    {
+        if(Val == codes[i])
+        {
+            Position = i;
+            break;
+        }
+        else i++;
+    }
+}
+//*****************************************************/
+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);
+    
+    Read_Encoder();
+    PC.printf("%d\n",data);
+    Get_EnValue(data);
+    PC.printf("%d\n********************\n",Position);    
+    
+    while(Position != SetPoint)
+    {
+        LU_PID.setProcessValue(Position);
+        LeftUpper.speed(LU_PID.compute());
+        
+        Read_Encoder();
+        Get_EnValue(data);
+    }
+    
+       
+}
+    
+    
+    
+    
+
+