Robotics ^___^ / Mbed 2 deprecated Robotics_LAB_motor_Done

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

main.cpp@0:74ea99c4db88, 2017-02-23 (annotated)

Committer:
hoting
Date:
Thu Feb 23 12:35:53 2017 +0000
Revision:
0:74ea99c4db88
Child:
1:657bebc0fe37
Robotics_LAB_motor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hoting 0:74ea99c4db88 1 #include "mbed.h"
hoting 0:74ea99c4db88 2
hoting 0:74ea99c4db88 3 //The number will be compiled as type "double" in default
hoting 0:74ea99c4db88 4 //Add a "f" after the number can make it compiled as type "float"
hoting 0:74ea99c4db88 5 #define Ts 0.01f //period of timer1 (s)
hoting 0:74ea99c4db88 6
hoting 0:74ea99c4db88 7 Ticker timer1;
hoting 0:74ea99c4db88 8 // servo motor
hoting 0:74ea99c4db88 9 PwmOut servo_cmd(A0);
hoting 0:74ea99c4db88 10 // DC motor
hoting 0:74ea99c4db88 11 PwmOut pwm1(D7);
hoting 0:74ea99c4db88 12 PwmOut pwm1n(D11);
hoting 0:74ea99c4db88 13 PwmOut pwm2(D8);
hoting 0:74ea99c4db88 14 PwmOut pwm2n(A3);
hoting 0:74ea99c4db88 15
hoting 0:74ea99c4db88 16 // Motor1 sensor
hoting 0:74ea99c4db88 17 InterruptIn HallA(A1);
hoting 0:74ea99c4db88 18 InterruptIn HallB(A2);
hoting 0:74ea99c4db88 19 // Motor2 sensor
hoting 0:74ea99c4db88 20 InterruptIn HallA_2(D13);
hoting 0:74ea99c4db88 21 InterruptIn HallB_2(D12);
hoting 0:74ea99c4db88 22
hoting 0:74ea99c4db88 23 // 函式宣告
hoting 0:74ea99c4db88 24 void init_IO();
hoting 0:74ea99c4db88 25 void init_TIMER();
hoting 0:74ea99c4db88 26 void timer1_ITR();
hoting 0:74ea99c4db88 27 void init_CN();
hoting 0:74ea99c4db88 28 void CN_ITR();
hoting 0:74ea99c4db88 29 void init_PWM();
hoting 0:74ea99c4db88 30
hoting 0:74ea99c4db88 31 // servo motor
hoting 0:74ea99c4db88 32 float servo_duty = 0.025; // 0.069 +(0.088/180)*angle, -90<angle<90
hoting 0:74ea99c4db88 33 // 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113
hoting 0:74ea99c4db88 34 int angle = 0;
hoting 0:74ea99c4db88 35
hoting 0:74ea99c4db88 36 // Hall sensor
hoting 0:74ea99c4db88 37 int HallA_1_state = 0;
hoting 0:74ea99c4db88 38 int HallB_1_state = 0;
hoting 0:74ea99c4db88 39 int state_1 = 0;
hoting 0:74ea99c4db88 40 int state_1_old = 0;
hoting 0:74ea99c4db88 41 int HallA_2_state = 0;
hoting 0:74ea99c4db88 42 int HallB_2_state = 0;
hoting 0:74ea99c4db88 43 int state_2 = 0;
hoting 0:74ea99c4db88 44 int state_2_old = 0;
hoting 0:74ea99c4db88 45
hoting 0:74ea99c4db88 46 // DC motor rotation speed control
hoting 0:74ea99c4db88 47 int speed_count_1 = 0;
hoting 0:74ea99c4db88 48 float rotation_speed_1 = 0.0;
hoting 0:74ea99c4db88 49 float rotation_speed_ref_1 = 0;
hoting 0:74ea99c4db88 50 float pwm1_duty = 0.5;
hoting 0:74ea99c4db88 51 float PI_out_1 = 0.0;
hoting 0:74ea99c4db88 52 float err_1 = 0.0;
hoting 0:74ea99c4db88 53 float ierr_1 = 0.0;
hoting 0:74ea99c4db88 54 int speed_count_2 = 0;
hoting 0:74ea99c4db88 55 float rotation_speed_2 = 0.0;
hoting 0:74ea99c4db88 56 float rotation_speed_ref_2 = 0;
hoting 0:74ea99c4db88 57 float pwm2_duty = 0.5;
hoting 0:74ea99c4db88 58 float PI_out_2 = 0.0;
hoting 0:74ea99c4db88 59 float err_2 = 0.0;
hoting 0:74ea99c4db88 60 float ierr_2 = 0.0;
hoting 0:74ea99c4db88 61
hoting 0:74ea99c4db88 62 int main()
hoting 0:74ea99c4db88 63 {
hoting 0:74ea99c4db88 64 init_TIMER();
hoting 0:74ea99c4db88 65 init_PWM();
hoting 0:74ea99c4db88 66 init_CN();
hoting 0:74ea99c4db88 67
hoting 0:74ea99c4db88 68 while(1) {
hoting 0:74ea99c4db88 69 }
hoting 0:74ea99c4db88 70 }
hoting 0:74ea99c4db88 71
hoting 0:74ea99c4db88 72 void init_TIMER()
hoting 0:74ea99c4db88 73 {
hoting 0:74ea99c4db88 74 timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (10000 micro-seconds)
hoting 0:74ea99c4db88 75 }
hoting 0:74ea99c4db88 76
hoting 0:74ea99c4db88 77 void init_PWM()
hoting 0:74ea99c4db88 78 {
hoting 0:74ea99c4db88 79 servo_cmd.period_ms(20);
hoting 0:74ea99c4db88 80 servo_cmd.write(servo_duty);
hoting 0:74ea99c4db88 81
hoting 0:74ea99c4db88 82 pwm1.period_us(50);
hoting 0:74ea99c4db88 83 pwm1.write(0.5);
hoting 0:74ea99c4db88 84 TIM1->CCER |= 0x4;
hoting 0:74ea99c4db88 85
hoting 0:74ea99c4db88 86 pwm2.period_us(50);
hoting 0:74ea99c4db88 87 pwm2.write(0.5);
hoting 0:74ea99c4db88 88 TIM1->CCER |= 0x40;
hoting 0:74ea99c4db88 89 }
hoting 0:74ea99c4db88 90 void init_CN()
hoting 0:74ea99c4db88 91 {
hoting 0:74ea99c4db88 92 HallA.rise(&CN_ITR);
hoting 0:74ea99c4db88 93 HallA.fall(&CN_ITR);
hoting 0:74ea99c4db88 94 HallB.rise(&CN_ITR);
hoting 0:74ea99c4db88 95 HallB.fall(&CN_ITR);
hoting 0:74ea99c4db88 96
hoting 0:74ea99c4db88 97 HallA_2.rise(&CN_ITR);
hoting 0:74ea99c4db88 98 HallA_2.fall(&CN_ITR);
hoting 0:74ea99c4db88 99 HallB_2.rise(&CN_ITR);
hoting 0:74ea99c4db88 100 HallB_2.fall(&CN_ITR);
hoting 0:74ea99c4db88 101 }
hoting 0:74ea99c4db88 102
hoting 0:74ea99c4db88 103 void CN_ITR()
hoting 0:74ea99c4db88 104 {
hoting 0:74ea99c4db88 105 // motor1
hoting 0:74ea99c4db88 106 HallA_1_state = HallA.read();
hoting 0:74ea99c4db88 107 HallB_1_state = HallB.read();
hoting 0:74ea99c4db88 108
hoting 0:74ea99c4db88 109 ///code for state determination///
hoting 0:74ea99c4db88 110
hoting 0:74ea99c4db88 111
hoting 0:74ea99c4db88 112
hoting 0:74ea99c4db88 113 //////////////////////////////////
hoting 0:74ea99c4db88 114
hoting 0:74ea99c4db88 115 //forward : speed_count_1 + 1
hoting 0:74ea99c4db88 116 //backward : speed_count_1 - 1
hoting 0:74ea99c4db88 117
hoting 0:74ea99c4db88 118
hoting 0:74ea99c4db88 119 // motor2
hoting 0:74ea99c4db88 120 HallA_2_state = HallA_2.read();
hoting 0:74ea99c4db88 121 HallB_2_state = HallB_2.read();
hoting 0:74ea99c4db88 122
hoting 0:74ea99c4db88 123 ///code for state determination///
hoting 0:74ea99c4db88 124
hoting 0:74ea99c4db88 125
hoting 0:74ea99c4db88 126
hoting 0:74ea99c4db88 127 //////////////////////////////////
hoting 0:74ea99c4db88 128
hoting 0:74ea99c4db88 129 //forward : speed_count_2 + 1
hoting 0:74ea99c4db88 130 //backward : speed_count_2 - 1
hoting 0:74ea99c4db88 131 }
hoting 0:74ea99c4db88 132
hoting 0:74ea99c4db88 133 void timer1_ITR()
hoting 0:74ea99c4db88 134 {
hoting 0:74ea99c4db88 135 // servo motor
hoting 0:74ea99c4db88 136 ///code for sevo motor///
hoting 0:74ea99c4db88 137
hoting 0:74ea99c4db88 138
hoting 0:74ea99c4db88 139
hoting 0:74ea99c4db88 140 /////////////////////////
hoting 0:74ea99c4db88 141
hoting 0:74ea99c4db88 142 if(servo_duty >= 0.113f)servo_duty = 0.113;
hoting 0:74ea99c4db88 143 else if(servo_duty <= 0.025f)servo_duty = 0.025;
hoting 0:74ea99c4db88 144 servo_cmd.write(servo_duty);
hoting 0:74ea99c4db88 145
hoting 0:74ea99c4db88 146 // motor1
hoting 0:74ea99c4db88 147 rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
hoting 0:74ea99c4db88 148 speed_count_1 = 0;
hoting 0:74ea99c4db88 149
hoting 0:74ea99c4db88 150 ///PI controller for motor1///
hoting 0:74ea99c4db88 151
hoting 0:74ea99c4db88 152
hoting 0:74ea99c4db88 153
hoting 0:74ea99c4db88 154 //////////////////////////////
hoting 0:74ea99c4db88 155
hoting 0:74ea99c4db88 156 if(PI_out_1 >= 0.5f)PI_out_1 = 0.5;
hoting 0:74ea99c4db88 157 else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5;
hoting 0:74ea99c4db88 158 pwm1_duty = PI_out_1 + 0.5f;
hoting 0:74ea99c4db88 159 pwm1.write(pwm1_duty);
hoting 0:74ea99c4db88 160 TIM1->CCER |= 0x4;
hoting 0:74ea99c4db88 161
hoting 0:74ea99c4db88 162 //motor2
hoting 0:74ea99c4db88 163 rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
hoting 0:74ea99c4db88 164 speed_count_2 = 0;
hoting 0:74ea99c4db88 165
hoting 0:74ea99c4db88 166 ///PI controller for motor2///
hoting 0:74ea99c4db88 167
hoting 0:74ea99c4db88 168
hoting 0:74ea99c4db88 169
hoting 0:74ea99c4db88 170 //////////////////////////////
hoting 0:74ea99c4db88 171
hoting 0:74ea99c4db88 172 if(PI_out_2 >= 0.5f)PI_out_2 = 0.5;
hoting 0:74ea99c4db88 173 else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5;
hoting 0:74ea99c4db88 174 pwm2_duty = PI_out_2 + 0.5f;
hoting 0:74ea99c4db88 175 pwm2.write(pwm2_duty);
hoting 0:74ea99c4db88 176 TIM1->CCER |= 0x40;
hoting 0:74ea99c4db88 177 }