Diff: main.cpp

Revision:

0:81b42d622dc5

Child:

1:8fb27fda4ec5

diff -r 000000000000 -r 81b42d622dc5 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu May 26 12:30:18 2016 +0000
@@ -0,0 +1,255 @@
+#include "mbed.h"
+ 
+#define Ts 0.01f    //period of timer1 (s)
+#define Kp 0.006f
+#define Ki 0.02f
+#define k1 1.02f
+#define k2 1.02f
+
+PwmOut pwm1(D7);
+PwmOut pwm1n(D11);
+PwmOut pwm2(D8);
+PwmOut pwm2n(A3);
+ 
+DigitalOut led1(A4);
+DigitalOut led2(A5);
+ 
+//Motor1 sensor
+InterruptIn HallA_1(A1);
+InterruptIn HallB_1(A2);
+//Motor2 sensor
+InterruptIn HallA_2(D13);
+InterruptIn HallB_2(D12);
+ 
+Serial bluetooth (D10,D2);
+Serial pc(D1,D0);
+ 
+Ticker timer1;
+void timer1_interrupt(void);
+void CN_interrupt(void);
+void BT_interrupt(void);
+ 
+void init_TIMER(void);
+void init_PWM(void);
+void init_CN(void);
+ 
+int8_t stateA_1=0, stateB_1=0, stateA_2=0, stateB_2=0;
+int8_t stateA_1_old = 0, stateB_1_old = 0, stateA_2_old = 0, stateB_2_old = 0;
+ 
+int c = 0; 
+int d = 0; 
+int v1Count = 0;
+int v2Count = 0;
+
+/*char Receive_Data;*/
+char Receive_Data[10];
+int Position_Error,Degree_Error;
+int Command_Flag;
+float sigma,delta,fSpeedRef_1,fSpeedRef_2;
+
+ 
+float v1 = 0.0, v1_ref = 0.0;
+float v1_err = 0.0, v1_ierr = 0.0, PIout_1 = 0.0, PIout_1_old = 0.0;
+float v2 = 0.0, v2_ref = 0.0;
+float v2_err = 0.0, v2_ierr = 0.0, PIout_2 = 0.0, PIout_2_old = 0.0;
+ 
+int main() 
+{
+    init_TIMER();
+    init_PWM();
+    init_CN();
+    
+    bluetooth.baud(115200);
+    pc.baud(57600);
+    
+    while(1)    
+    {
+        if(bluetooth.readable())
+        {
+            pc.attach(&BT_interrupt);
+
+            if(Command_Flag==1)
+            {
+                Position_Error=(Receive_Data[2]-0x30)*100+(Receive_Data[3]-0x30)*10+(Receive_Data[4]-0x30); 
+                
+                if(Receive_Data[1]=='-')
+                    Position_Error=-1*Position_Error;
+                else 
+                    Position_Error=Position_Error; 
+                    
+                Degree_Error=(Receive_Data[6]-0x30)*100+(Receive_Data[7]-0x30)*10+(Receive_Data[8]-0x30);
+                
+                if(Receive_Data[5]=='-')
+                    Degree_Error=-1*Degree_Error;
+                else 
+                    Degree_Error=Degree_Error;
+                
+      
+                sigma = k1*Position_Error;
+                delta = k2*Degree_Error;
+                
+                if (sigma > 500){sigma = 500;}
+                if (sigma < -500){sigma = -500;}
+                if (delta > 500){delta = 500;}
+                if (delta < -500){delta = -500;}
+                fSpeedRef_1 = sigma + delta;
+                fSpeedRef_2 = sigma - delta;
+                v1_ref = fSpeedRef_1;
+                v2_ref = -1*fSpeedRef_2;
+            }
+        }
+    }
+}
+ 
+void timer1_interrupt(void)
+{
+    //Motor 1
+    v1 = (float)v1Count * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
+    v1Count = 0;
+    
+    ///code for PI control///
+    
+    v1_err = v1_ref - v1;
+    v1_ierr = v1_ierr + v1_err*Ts;
+    PIout_1 = Kp*v1_err + Ki*v1_ierr;
+    
+    /////////////////////////
+    
+    if(PIout_1 >= 0.5f)PIout_1 = 0.5f;
+    else if(PIout_1 <= -0.5f)PIout_1 = -0.5f;
+    pwm1.write(PIout_1 + 0.5f);
+    TIM1->CCER |= 0x4;
+    
+    
+    //Motor 2
+    v2 = (float)v2Count * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
+    v2Count = 0;
+    
+    ///code for PI control///
+    
+    v2_err = v2_ref - v2;
+    v2_ierr = v2_ierr + v2_err*Ts;
+    PIout_2 = Kp*v2_err + Ki*v2_ierr;
+    
+    /////////////////////////
+    
+    if(PIout_2 >= 0.5f)PIout_2 = 0.5f;
+    else if(PIout_2 <= -0.5f)PIout_2 = -0.5f;
+    pwm2.write(PIout_2 + 0.5f);
+    TIM1->CCER |= 0x40;
+    
+
+}
+ 
+void CN_interrupt(void)
+{
+    //Motor 1
+    stateA_1 = HallA_1.read();
+    stateB_1 = HallB_1.read();
+    
+    ///code for state determination///
+    
+    if(stateA_1 == stateB_1_old && c == 1)
+    {
+        v1Count = v1Count + 1;
+    }
+        
+    if(stateB_1 == stateA_1_old && c == 1) 
+    {
+        v1Count = v1Count - 1;
+    }
+    
+    c = 1;
+    stateA_1_old = stateA_1;
+    stateB_1_old = stateB_1;
+    
+    //////////////////////////////////
+    
+    
+    //Motor 2
+    stateA_2 = HallA_2.read();
+    stateB_2 = HallB_2.read();
+    
+    ///code for state determination///
+    
+    if(stateA_2 == stateB_2_old && d == 1)
+    {
+        v2Count = v2Count + 1;
+    }
+        
+    if(stateB_2 == stateA_2_old && d == 1) 
+    {
+        v2Count = v2Count - 1;
+    }
+    
+    d = 1;
+    stateA_2_old = stateA_2;
+    stateB_2_old = stateB_2;
+    
+    //////////////////////////////////
+    
+}
+ 
+void init_TIMER(void)
+{
+    timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
+}         
+   
+void init_PWM(void)
+{
+    pwm1.period_us(50);
+    pwm1.write(0.5);
+    TIM1->CCER |= 0x4;
+    
+    pwm2.period_us(50);
+    pwm2.write(0.5);
+    TIM1->CCER |= 0x40;
+}
+ 
+void init_CN(void)
+{
+    HallA_1.rise(&CN_interrupt);
+    HallA_1.fall(&CN_interrupt);
+    HallB_1.rise(&CN_interrupt);
+    HallB_1.fall(&CN_interrupt);
+    
+    HallA_2.rise(&CN_interrupt);
+    HallA_2.fall(&CN_interrupt);
+    HallB_2.rise(&CN_interrupt);
+    HallB_2.fall(&CN_interrupt);
+    
+    stateA_1 = HallA_1.read();
+    stateB_1 = HallB_1.read();
+    stateA_2 = HallA_2.read();
+    stateB_2 = HallB_2.read();
+}
+
+void BT_interrupt(void)
+{
+    int Receive_Counter,Receive_Flag;
+    
+    static char Temp;
+    Temp = bluetooth.getc(); 
+    
+    if (Receive_Flag==1)
+    {
+        Receive_Counter++;
+        Receive_Data[Receive_Counter]=Temp;
+     
+        if(Receive_Counter==9)
+        {
+            Command_Flag=1;
+            Receive_Flag=0;
+            Receive_Counter=0;
+        }
+    }
+    else
+    {
+        if(Temp==36)
+        {
+            Receive_Flag=1;
+            Receive_Counter=0;
+            Receive_Data[0]= Temp;
+        }
+    }
+}