Diff: ID_motor.cpp

Revision:

0:aa69084e1cae

Child:

1:307e6821d26f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ID_motor.cpp	Mon Mar 20 15:32:29 2017 +0000
@@ -0,0 +1,130 @@
+#include "mbed.h"
+ 
+ // numbers will be defined as double in default, add f after numbers will denfined it as float
+ // IDEA: to observe and control two motors seperatedly 
+ 
+ #define Ts = 0.01f  // sampling interval ( sec.)
+ 
+ Serial pc(USBTX, USBRX) ;  // serial (tx, rx) 
+ Ticker timer1 ;            // Timer
+ 
+ // DC motor complement PWM parameter 
+ PwmOut pwm1 (D7) ;
+ PwmOut pwm1n (D11) ;
+ PwmOut pwm2 (D8) ;
+ PwmOut pwm2n (A3) ;
+
+// Hall sensor for motor 1
+InterruptIn HallA_1 (A1) ;
+InterruptIn HallB_1 (A2) ; 
+int HallA_1_state = 0 ;
+int HallB_1_state = 0 ;
+
+// Hall sensor for motor 2
+InterruptIn HallA_2 (D13) ;
+InterruptIn HallB_2 (D12) ;
+int HallA_2_state = 0 ;
+int HallB_2_state = 0 ;
+
+
+// defination of functions 
+void init_I0() ;
+void init_TIMER() ;
+void timer1_ITR() ;
+void init_CN() ;
+void CN_ITR_1() ;
+void CN_ITR_2() ;
+void init_PWM() ;
+
+// Speed estimator for DC motor
+int speed_count_1 = 0 ;
+int speed_count_2 = 0 ; 
+float rotation_speed_1 = 0.0 ;
+float rotation_speed_2 = 0.0 ;
+float pwm1_duty = 0.5 ;
+float pwm2_duty = 0.5 ; 
+
+
+
+
+int main() 
+{
+    init_TIMER() ;
+    init_PWM() ;
+    init_CN() ;
+    
+    while(true)
+    {
+    }
+}
+
+void init_TIMER()
+{
+ timer1.attach_us(&timer1_ITR, 10000.0);   // address of function to be attached (timer1_ITR) and interval (in us)   
+}
+
+void init_PWM()
+{
+ // INPUT SIG. to motor 1
+ pwm1_duty = 0.7f ;
+ pwm1.write(pwm1_duty) ;  // Volts = 5* pwm1_duty (volts)
+ TIM1->CCER |= 0x4 ;      // Complement PWM
+ 
+ // INPUT SIG. to motor 2
+ pwm2_duty = 0.7f ;
+ pwm2.write(pwm2_duty) ;  // Volts = 5* pwm1_duty (volts)
+ TIM1->CCER |= 0x40 ;      // Complement PWM
+}
+
+
+
+void init_CN()        // Rising and Falling Trig for ONLY ONE HALL sensor 
+{
+   HallA_1.rise(&CN_ITR_1); 
+   HallA_1.fall(&CN_ITR_1);
+   
+   HallA_2.rise(&CN_ITR_2);
+   HallA_2.fall(&CN_ITR_2);
+    
+}
+
+void CN_ITR_1()   // judge direction and speed_count for motor 1
+{   
+    // read wat hall measured
+    HallA_1_state = HallA_1.read() ;    
+    HallB_1_state = HallB_1.read() ;
+    
+    // to judge rotation direction
+    if (HallA_1_state == HallB_1_state)
+    speed_count_1 += 1 ;
+    else
+    speed_count_1 -= 1 ;
+    
+}
+
+void CN_ITR_2()    // judge direction and speed_count for motor 2
+{
+  // read wat hall measured
+  HallA_2_state = HallA_2.read() ;
+  HallB_2_state = HallB_2.read() ;
+  
+  // judge rotation direction
+  if (HallA_2_state == HallB_2_state)
+  speed_count_2 += 1 ;
+  else 
+  speed_count_2 -= 1 ;
+     
+}
+
+void timer1_ITR()
+{
+ // monitor for motor 1
+ rotation_speed_1 = (float)speed_count_1*100.0f/ 12.0f* 60.0f/ 29.0f ; // unit: rpm
+ speed_count_1 = 0 ;                                                   // initialize for another time
+ pc.printf("Wm1 = %f (rpm) \n", rotation_speed_1) ;                        // Serial print to know Wm    
+    
+// monitor for motor 2
+rotation_speed_2 = (float)speed_count_2*100.0f/ 12.0f* 60.0f/ 29.0f ; // unit: rpm
+speed_count_2 = 0 ;                                                   // initialize for next time
+pc.printf("Wm2 = %f (rpm) \n", rotation_speed_2) ;                    // Serial print to know Wm
+}
\ No newline at end of file