Robotics Term Project
/
Robottics_Motion
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Diff: main.cpp
- Revision:
- 0:2ac10e7b6b03
- Child:
- 8:079c3326816e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue May 24 05:29:47 2016 +0000
@@ -0,0 +1,283 @@
+/*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.0025f
+#define Ki 0.008f
+
+PwmOut pwm1(D7);
+PwmOut pwm1n(D11);
+PwmOut pwm2(D8);
+PwmOut pwm2n(A3);
+PwmOut servo(A0);
+
+Serial bluetooth(D10,D2);
+Serial pc(D1, D0);
+
+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);
+
+Ticker timer1;
+void timer1_interrupt(void);
+void CN_interrupt(void);
+
+void init_TIMER(void);
+void init_PWM(void);
+void init_CN(void);
+void init_err(void);
+
+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;
+
+int v1Count = 0;
+int v2Count = 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;
+
+float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90
+
+int main() {
+
+ init_TIMER();
+ init_PWM();
+ init_CN();
+
+ bluetooth.baud(115200); //設定鮑率
+ pc.baud(57600);
+
+
+ while(1)
+ {
+ if(pc.readable())
+ {
+ bluetooth.putc(pc.getc());
+ }
+ if(bluetooth.readable())
+ {
+ pc.putc(bluetooth.getc());
+ }
+ }
+}
+
+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 = Ts*v1_err + v1_ierr;
+ 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 = Ts*v2_err + v2_ierr;
+ 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;
+
+ switch(bluetooth.getc())
+ {
+ case '1':
+ v1_ref = 30;
+ v2_ref = 30;
+ init_err();
+ break;
+ case '2':
+ v1_ref = 40;
+ v2_ref = 40;
+ init_err();
+ break;
+ }
+
+ // Servo
+
+ servo_duty = 0.079 + (0.084/180)*angle;
+ servo.write(servo_duty);
+ servo = 1;
+ wait(0.1);
+ servo = 0;
+
+ ////////////////////////////////////////////
+
+ if(servo_duty >= 0.121f)servo_duty = 0.121;
+ else if(servo_duty <= 0.037f)servo_duty = 0.037;
+ servo.write(servo_duty);
+
+}
+
+void CN_interrupt(void)
+{
+ //Motor 1
+ stateA_1 = HallA_1.read();
+ stateB_1 = HallB_1.read();
+
+ ///code for state determination///
+ if(stateA_1==0&&stateB_1==0){
+ state_1 = 1;}
+ else if(stateA_1==0&&stateB_1==1){
+ state_1 = 2;}
+ else if(stateA_1==1&&stateB_1==1){
+ state_1 = 3;}
+ else if(stateA_1==1&&stateB_1==0){
+ state_1 = 4;}
+
+ if(state_1 == 1)
+ {
+ if(state_1-state_1_old == -3)
+ v1Count=v1Count+1;
+ else if(state_1-state_1_old == -1)
+ v1Count=v1Count-1;
+ }
+ else if(state_1 == 2)
+ {
+ if(state_1-state_1_old == 1)
+ v1Count=v1Count+1;
+ else if(state_1-state_1_old == -1)
+ v1Count=v1Count-1;
+ }
+ else if(state_1 == 3)
+ {
+ if(state_1-state_1_old == 1)
+ v1Count=v1Count+1;
+ else if(state_1-state_1_old == -1)
+ v1Count=v1Count-1;
+ }
+ else if(state_1 == 4)
+ {
+ if(state_1-state_1_old == 1)
+ v1Count=v1Count+1;
+ else if(state_1-state_1_old == 3)
+ v1Count=v1Count-1;
+ }
+ state_1_old = state_1;
+
+
+ //////////////////////////////////
+
+ //Forward
+ //v1Count +1
+ //Inverse
+ //v1Count -1
+
+
+ //Motor 2
+ stateA_2 = HallA_2.read();
+ stateB_2 = HallB_2.read();
+
+ ///code for state determination///
+ if(stateA_2==0&&stateB_2==0){
+ state_2 = 1;}
+ else if(stateA_2==0&&stateB_2==1){
+ state_2 = 2;}
+ else if(stateA_2==1&&stateB_2==1){
+ state_2 = 3;}
+ else if(stateA_2==1&&stateB_2==0){
+ state_2 = 4;}
+
+ if(state_2 == 1)
+ {
+ if(state_2-state_2_old == -3)
+ v2Count=v2Count+1;
+ else if(state_2-state_2_old == -1)
+ v2Count=v2Count-1;
+ }
+ else if(state_2 == 2)
+ {
+ if(state_2-state_2_old == 1)
+ v2Count=v2Count+1;
+ else if(state_2-state_2_old == -1)
+ v2Count=v2Count-1;
+ }
+ else if(state_2 == 3)
+ {
+ if(state_2-state_2_old == 1)
+ v2Count=v2Count+1;
+ else if(state_2-state_2_old == -1)
+ v2Count=v2Count-1;
+ }
+ else if(state_2 == 4)
+ {
+ if(state_2-state_2_old == 1)
+ v2Count=v2Count+1;
+ else if(state_2-state_2_old == 3)
+ v2Count=v2Count-1;
+ }
+ state_2_old = state_2;
+
+ //////////////////////////////////
+
+ //Forward
+ //v2Count +1
+ //Inverse
+ //v2Count -1
+}
+
+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 init_err(void)
+{
+ v1_ierr = 0.0;
+ v2_ierr = 0.0;
+}
\ No newline at end of file