Revision 0:0971f0666990, committed 2016-04-07

Comitter:

YCTung

Date:

Thu Apr 07 22:53:48 2016 +0000

Child:

1:6747911cdd90

Commit message:

Robotics_Lab_DCMotor

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 07 22:53:48 2016 +0000
@@ -0,0 +1,155 @@
+/*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)
+
+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 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 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;
+
+int main() {
+    
+    init_TIMER();
+    init_PWM();
+    init_CN();
+    
+    v1_ref = 0.0;
+    v2_ref = 0.0;
+    
+    while(1) {
+        ;
+    }
+}
+
+void timer1_interrupt(void)
+{
+    //Motor 1
+    v1 = (float)v1Count * 100.0f / 12.0f * 60.0f / 29.0f;
+    v1Count = 0;
+    
+    ///code for PI control///
+    
+    
+    /////////////////////////
+    
+    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;
+    v2Count = 0;
+    
+    ///code for PI control///
+    
+    
+    /////////////////////////
+    
+    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);
+    pwm2.write(0);
+    TIM1->CCER |= 0x40;
+}
+
+void CN_interrupt(void)
+{
+    //Motor 1
+    stateA_1 = HallA_1.read();
+    stateB_1 = HallB_1.read();
+    
+    ///code for state determination///
+    
+    
+    //////////////////////////////////
+    
+    //Forward
+    //v1Count +1
+    //Inverse
+    //v1Count -1
+    
+    
+    //Motor 2
+    stateA_2 = HallA_2.read();
+    stateB_2 = HallB_2.read();
+    
+    ///code for state determination///
+    
+    
+    //////////////////////////////////
+    
+    //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();
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Apr 07 22:53:48 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/99a22ba036c9
\ No newline at end of file