PME_Police / Mbed 2 deprecated Identify_DCmotor

Bring down Resolution to have a easier code. We could give it a shot. I think it will help our Identification of DC motor for it can run faster (maybe).

Dependencies:   mbed

ID_motor.cpp@1:307e6821d26f, 2017-03-20 (annotated)

Committer:
bobolee1239
Date:
Mon Mar 20 15:49:39 2017 +0000
Revision:
1:307e6821d26f
Parent:
0:aa69084e1cae 
Motor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bobolee1239 0:aa69084e1cae 1 #include "mbed.h"
bobolee1239 0:aa69084e1cae 2
bobolee1239 0:aa69084e1cae 3 // numbers will be defined as double in default, add f after numbers will denfined it as float
bobolee1239 0:aa69084e1cae 4 // IDEA: to observe and control two motors seperatedly
bobolee1239 0:aa69084e1cae 5
bobolee1239 0:aa69084e1cae 6 #define Ts = 0.01f // sampling interval ( sec.)
bobolee1239 0:aa69084e1cae 7
bobolee1239 0:aa69084e1cae 8 Serial pc(USBTX, USBRX) ; // serial (tx, rx)
bobolee1239 0:aa69084e1cae 9 Ticker timer1 ; // Timer
bobolee1239 0:aa69084e1cae 10
bobolee1239 0:aa69084e1cae 11 // DC motor complement PWM parameter
bobolee1239 0:aa69084e1cae 12 PwmOut pwm1 (D7) ;
bobolee1239 0:aa69084e1cae 13 PwmOut pwm1n (D11) ;
bobolee1239 0:aa69084e1cae 14 PwmOut pwm2 (D8) ;
bobolee1239 0:aa69084e1cae 15 PwmOut pwm2n (A3) ;
bobolee1239 0:aa69084e1cae 16
bobolee1239 0:aa69084e1cae 17 // Hall sensor for motor 1
bobolee1239 0:aa69084e1cae 18 InterruptIn HallA_1 (A1) ;
bobolee1239 0:aa69084e1cae 19 InterruptIn HallB_1 (A2) ;
bobolee1239 0:aa69084e1cae 20 int HallA_1_state = 0 ;
bobolee1239 0:aa69084e1cae 21 int HallB_1_state = 0 ;
bobolee1239 0:aa69084e1cae 22
bobolee1239 0:aa69084e1cae 23 // Hall sensor for motor 2
bobolee1239 0:aa69084e1cae 24 InterruptIn HallA_2 (D13) ;
bobolee1239 0:aa69084e1cae 25 InterruptIn HallB_2 (D12) ;
bobolee1239 0:aa69084e1cae 26 int HallA_2_state = 0 ;
bobolee1239 0:aa69084e1cae 27 int HallB_2_state = 0 ;
bobolee1239 0:aa69084e1cae 28
bobolee1239 0:aa69084e1cae 29
bobolee1239 0:aa69084e1cae 30 // defination of functions
bobolee1239 0:aa69084e1cae 31 void init_I0() ;
bobolee1239 0:aa69084e1cae 32 void init_TIMER() ;
bobolee1239 0:aa69084e1cae 33 void timer1_ITR() ;
bobolee1239 0:aa69084e1cae 34 void init_CN() ;
bobolee1239 0:aa69084e1cae 35 void CN_ITR_1() ;
bobolee1239 0:aa69084e1cae 36 void CN_ITR_2() ;
bobolee1239 0:aa69084e1cae 37 void init_PWM() ;
bobolee1239 0:aa69084e1cae 38
bobolee1239 0:aa69084e1cae 39 // Speed estimator for DC motor
bobolee1239 0:aa69084e1cae 40 int speed_count_1 = 0 ;
bobolee1239 0:aa69084e1cae 41 int speed_count_2 = 0 ;
bobolee1239 0:aa69084e1cae 42 float rotation_speed_1 = 0.0 ;
bobolee1239 0:aa69084e1cae 43 float rotation_speed_2 = 0.0 ;
bobolee1239 0:aa69084e1cae 44 float pwm1_duty = 0.5 ;
bobolee1239 0:aa69084e1cae 45 float pwm2_duty = 0.5 ;
bobolee1239 0:aa69084e1cae 46
bobolee1239 0:aa69084e1cae 47
bobolee1239 0:aa69084e1cae 48
bobolee1239 0:aa69084e1cae 49
bobolee1239 0:aa69084e1cae 50 int main()
bobolee1239 0:aa69084e1cae 51 {
bobolee1239 0:aa69084e1cae 52 init_TIMER() ;
bobolee1239 0:aa69084e1cae 53 init_PWM() ;
bobolee1239 0:aa69084e1cae 54 init_CN() ;
bobolee1239 0:aa69084e1cae 55
bobolee1239 0:aa69084e1cae 56 while(true)
bobolee1239 0:aa69084e1cae 57 {
bobolee1239 0:aa69084e1cae 58 }
bobolee1239 0:aa69084e1cae 59 }
bobolee1239 0:aa69084e1cae 60
bobolee1239 0:aa69084e1cae 61 void init_TIMER()
bobolee1239 0:aa69084e1cae 62 {
bobolee1239 0:aa69084e1cae 63 timer1.attach_us(&timer1_ITR, 10000.0); // address of function to be attached (timer1_ITR) and interval (in us)
bobolee1239 0:aa69084e1cae 64 }
bobolee1239 0:aa69084e1cae 65
bobolee1239 0:aa69084e1cae 66 void init_PWM()
bobolee1239 0:aa69084e1cae 67 {
bobolee1239 0:aa69084e1cae 68 // INPUT SIG. to motor 1
bobolee1239 0:aa69084e1cae 69 pwm1_duty = 0.7f ;
bobolee1239 0:aa69084e1cae 70 pwm1.write(pwm1_duty) ; // Volts = 5* pwm1_duty (volts)
bobolee1239 0:aa69084e1cae 71 TIM1->CCER |= 0x4 ; // Complement PWM
bobolee1239 0:aa69084e1cae 72
bobolee1239 0:aa69084e1cae 73 // INPUT SIG. to motor 2
bobolee1239 0:aa69084e1cae 74 pwm2_duty = 0.7f ;
bobolee1239 0:aa69084e1cae 75 pwm2.write(pwm2_duty) ; // Volts = 5* pwm1_duty (volts)
bobolee1239 0:aa69084e1cae 76 TIM1->CCER |= 0x40 ; // Complement PWM
bobolee1239 0:aa69084e1cae 77 }
bobolee1239 0:aa69084e1cae 78
bobolee1239 0:aa69084e1cae 79
bobolee1239 0:aa69084e1cae 80
bobolee1239 0:aa69084e1cae 81 void init_CN() // Rising and Falling Trig for ONLY ONE HALL sensor
bobolee1239 0:aa69084e1cae 82 {
bobolee1239 0:aa69084e1cae 83 HallA_1.rise(&CN_ITR_1);
bobolee1239 0:aa69084e1cae 84 HallA_1.fall(&CN_ITR_1);
bobolee1239 0:aa69084e1cae 85
bobolee1239 0:aa69084e1cae 86 HallA_2.rise(&CN_ITR_2);
bobolee1239 0:aa69084e1cae 87 HallA_2.fall(&CN_ITR_2);
bobolee1239 0:aa69084e1cae 88
bobolee1239 0:aa69084e1cae 89 }
bobolee1239 0:aa69084e1cae 90
bobolee1239 0:aa69084e1cae 91 void CN_ITR_1() // judge direction and speed_count for motor 1
bobolee1239 0:aa69084e1cae 92 {
bobolee1239 0:aa69084e1cae 93 // read wat hall measured
bobolee1239 0:aa69084e1cae 94 HallA_1_state = HallA_1.read() ;
bobolee1239 0:aa69084e1cae 95 HallB_1_state = HallB_1.read() ;
bobolee1239 0:aa69084e1cae 96
bobolee1239 0:aa69084e1cae 97 // to judge rotation direction
bobolee1239 0:aa69084e1cae 98 if (HallA_1_state == HallB_1_state)
bobolee1239 0:aa69084e1cae 99 speed_count_1 += 1 ;
bobolee1239 0:aa69084e1cae 100 else
bobolee1239 0:aa69084e1cae 101 speed_count_1 -= 1 ;
bobolee1239 0:aa69084e1cae 102
bobolee1239 0:aa69084e1cae 103 }
bobolee1239 0:aa69084e1cae 104
bobolee1239 0:aa69084e1cae 105 void CN_ITR_2() // judge direction and speed_count for motor 2
bobolee1239 0:aa69084e1cae 106 {
bobolee1239 0:aa69084e1cae 107 // read wat hall measured
bobolee1239 0:aa69084e1cae 108 HallA_2_state = HallA_2.read() ;
bobolee1239 0:aa69084e1cae 109 HallB_2_state = HallB_2.read() ;
bobolee1239 0:aa69084e1cae 110
bobolee1239 0:aa69084e1cae 111 // judge rotation direction
bobolee1239 0:aa69084e1cae 112 if (HallA_2_state == HallB_2_state)
bobolee1239 0:aa69084e1cae 113 speed_count_2 += 1 ;
bobolee1239 0:aa69084e1cae 114 else
bobolee1239 0:aa69084e1cae 115 speed_count_2 -= 1 ;
bobolee1239 0:aa69084e1cae 116
bobolee1239 0:aa69084e1cae 117 }
bobolee1239 0:aa69084e1cae 118
bobolee1239 0:aa69084e1cae 119 void timer1_ITR()
bobolee1239 0:aa69084e1cae 120 {
bobolee1239 0:aa69084e1cae 121 // monitor for motor 1
bobolee1239 0:aa69084e1cae 122 rotation_speed_1 = (float)speed_count_1*100.0f/ 12.0f* 60.0f/ 29.0f ; // unit: rpm
bobolee1239 0:aa69084e1cae 123 speed_count_1 = 0 ; // initialize for another time
bobolee1239 0:aa69084e1cae 124 pc.printf("Wm1 = %f (rpm) \n", rotation_speed_1) ; // Serial print to know Wm
bobolee1239 1:307e6821d26f 125 // read Data to Matlab
bobolee1239 1:307e6821d26f 126 // pc.printf("%f ",rotation_speed_1);
bobolee1239 0:aa69084e1cae 127
bobolee1239 0:aa69084e1cae 128 // monitor for motor 2
bobolee1239 0:aa69084e1cae 129 rotation_speed_2 = (float)speed_count_2*100.0f/ 12.0f* 60.0f/ 29.0f ; // unit: rpm
bobolee1239 0:aa69084e1cae 130 speed_count_2 = 0 ; // initialize for next time
bobolee1239 0:aa69084e1cae 131 pc.printf("Wm2 = %f (rpm) \n", rotation_speed_2) ; // Serial print to know Wm
bobolee1239 1:307e6821d26f 132 // read Data to Matlab
bobolee1239 1:307e6821d26f 133 //pc.printf("%f ", rotation_speed_2) ;
bobolee1239 0:aa69084e1cae 134 }