Diff: main.cpp

Revision:

0:7a08321141df

Child:

1:c4f94a2d3cba

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed May 25 07:56:14 2016 +0000
@@ -0,0 +1,249 @@
+#include "mbed.h"
+ 
+#define Ts 0.01f    //period of timer1 (s)
+#define Kp 0.006f
+#define Ki 0.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 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 Command_Flag;
+/*char Receive_Data;*/
+char Receive_Data[10];
+int Position_Error,Degree_Error;
+int  pwm_duty,i;
+double k1,k2,sigma,delta,fSpeedRef_1,fSpeedRef_2;
+ 
+int c = 0; 
+int d = 0; 
+int v1Count = 0;
+int v2Count = 0;
+int counter = 0;
+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())
+        {
+        for(i=0;i<=9;i++)
+        Receive_Data[i] =bluetooth.getc(); 
+        ISR_U2RXInterrupt();
+            if(Command_Flag==1)
+            {
+            Position_Error=int((Receive_Data[2]-0x30))*100+int((Receive_Data[3]-0x30))*10+int((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;
+            if(Receive_Data[9]=='j') pwm_duty=148;
+            else if (Receive_Data[9]=='k') pwm_duty=100;
+            Command_Flag=0;
+            } 
+        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;
+    
+    ///code for refresh the bluetooth print///
+
+    /////////////////////////
+}
+ 
+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 ISR_U2RXInterrupt(void)
+{
+int Receive_Counter,Receive_Flag;
+
+static char Temp;
+Temp=U2RXREG;
+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;
+}
+else
+{}
+}
+IFS1bits.U2RXIF=0;
+}
\ No newline at end of file