2016 NTHU Robo
vr.1
Dependencies: mbed
Diff: main.cpp
- Revision:
- 0:9ed7b8e278fa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Apr 18 05:28:06 2016 +0000
@@ -0,0 +1,275 @@
+/*LAB_DCMotor*/
+#include "mbed.h"
+
+//The number will be compiled as type "double" in default
+//Add a "f" after the number can make it compiled as type "float"
+#define Ts 0.01f //period of timer1 (s)
+#define Kp 0.053f
+#define Ki 0.013f
+
+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 bt(D10,D2);
+
+Ticker timer1;
+void timer1_interrupt(void);
+void CN_interrupt(void);
+
+void init_TIMER(void);
+void init_PWM(void);
+void init_CN(void);
+
+void set_rpm(void);//bluetooth in
+
+int8_t stateA_1=0, stateB_1=0, stateA_2=0, stateB_2=0;
+int8_t state_1 = 0, state_1_old = 0, state_2 = 0, state_2_old = 0;
+int8_t state_1_dec = 0,state_2_dec = 0;//decide foward or inverse
+
+int v1Count = 0;
+int v2Count = 0;
+
+double v1 = 0.0, v1_ref = 0.0;
+double v1_err = 0.0, v1_ierr = 0.0, PIout_1 = 0.0, PIout_1_old = 0.0;
+double v2 = 0.0, v2_ref = 0.0;
+double v2_err = 0.0, v2_ierr = 0.0, PIout_2 = 0.0, PIout_2_old = 0.0;
+
+int sec_1 = 0, sec_2 = 0;
+//////////command/////////////
+char RL;
+double char_1;//藍芽讀到的第一位數字元
+double char_2;//藍芽讀到的第二位數字元
+
+double a;//個位數
+double b;//十位數
+//////////////////////////////
+
+int main() {
+
+ init_TIMER();
+ init_PWM();
+ init_CN();
+ bt.baud(115200);
+
+ v1_ref = 0.0;
+ v2_ref = 0.0;
+
+ while(1)
+ {
+
+ set_rpm();
+
+ }
+}
+
+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;
+ PIout_1 = Kp * v1_err + Ki * Ts * 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;
+ PIout_2 = Kp * v2_err + Ki * Ts * 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();
+
+ state_1 = stateA_1 * 2 + stateB_1;
+
+ state_1_dec = state_1 - state_1_old; //判別正反
+
+
+ ///code for state determination///
+ if(state_1_dec == 1 || state_1_dec == -3 ) //Forward
+ {
+ v1Count++;
+ }
+
+ else if(state_1_dec == -1 || state_1_dec == 3 ) //Inverse
+ {
+ v1Count--;
+ }
+
+ state_1_old = state_1;
+
+
+ //Motor 2
+ stateA_2 = HallA_2.read();
+ stateB_2 = HallB_2.read();
+
+ state_2 = stateA_2 * 2 + stateB_2;
+
+ state_2_dec = state_2 - state_2_old; //判別正反
+
+
+ ///code for state determination///
+ if(state_2_dec == 1 || state_2_dec == -3 ) //Forward
+ {
+ v1Count++;
+ }
+
+ else if(state_2_dec == -1 || state_2_dec == 3 ) //Inverse
+ {
+ v1Count--;
+ }
+
+ state_2_old = state_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 set_rpm(void)
+{
+
+ RL = bt.getc();
+
+ switch( RL )
+ {
+ case 'r':
+ bt.printf("%c",RL);
+
+ char_1 = bt.getc();
+ a = char_1 - 48;
+
+ if(a > -1 && a < 10)
+ {
+ char_2 = bt.getc();
+ b = char_2 - 48;
+ v1_ierr = 0;
+ v1_ref = 10*a+b;
+
+ bt.printf("%f\n",v1_ref);
+ }
+
+ else
+ {
+ bt.printf("please key in number");
+ }
+
+
+ while(v1 < v1_ref*0.95 || v1 > v1_ref*1.05 )
+ {
+ bt.printf("state %d ",state_1_dec);
+ bt.printf("%f ",v1);
+ sec_1++;
+ bt.printf("%d\n",sec_1);
+ wait(1);
+ }
+ sec_1 = 0;
+ bt.printf("end %d\n",sec_1);
+
+ break;
+ /////////////////////////////////
+
+ case 'l':
+ bt.printf("%c",RL);
+
+ char_1 = bt.getc();
+ a = char_1 - 48;
+
+ if(a > -1 && a < 10)
+ {
+ char_2 = bt.getc();
+ b = char_2 - 48;
+ v2_ierr = 0;
+ v2_ref = 10*a+b;
+
+ bt.printf("%f\n",v2_ref);
+ }
+
+ else
+ {
+ bt.printf("please key in number");
+ }
+
+
+ while(v2 < v2_ref*0.95 || v2 > v2_ref*1.05 )
+ {
+ bt.printf("%f ",v2);
+ sec_2++;
+ bt.printf("%d\n",sec_2);
+ wait(1);
+ }
+ sec_2 = 0;
+ bt.printf("end %d\n",sec_2);
+
+ break;
+ }
+
+}
+