PME_Police / Mbed 2 deprecated Robotics_LAB_motor

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

Fork of Robotics_LAB_motor by NTHUPME_Robotics_2017

main.cpp@1:13ce5b28f6dd, 2017-03-21 (annotated)

Committer:
ryan820909
Date:
Tue Mar 21 05:36:09 2017 +0000
Revision:
1:13ce5b28f6dd
Parent:
0:74ea99c4db88 
shit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hoting 0:74ea99c4db88 1 #include "mbed.h"
ryan820909 1:13ce5b28f6dd 2 Serial pc(USBTX,USBRX);
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();
ryan820909 1:13ce5b28f6dd 27 void init_CN_1();
ryan820909 1:13ce5b28f6dd 28 void init_CN_2();
ryan820909 1:13ce5b28f6dd 29 void CN_ITR_1();
ryan820909 1:13ce5b28f6dd 30 void CN_ITR_2();
hoting 0:74ea99c4db88 31 void init_PWM();
hoting 0:74ea99c4db88 32
hoting 0:74ea99c4db88 33 // servo motor
hoting 0:74ea99c4db88 34 float servo_duty = 0.025; // 0.069 +(0.088/180)*angle, -90<angle<90
hoting 0:74ea99c4db88 35 // 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113
hoting 0:74ea99c4db88 36 int angle = 0;
hoting 0:74ea99c4db88 37
hoting 0:74ea99c4db88 38 // Hall sensor
hoting 0:74ea99c4db88 39 int HallA_1_state = 0;
hoting 0:74ea99c4db88 40 int HallB_1_state = 0;
hoting 0:74ea99c4db88 41 int state_1 = 0;
hoting 0:74ea99c4db88 42 int state_1_old = 0;
hoting 0:74ea99c4db88 43 int HallA_2_state = 0;
hoting 0:74ea99c4db88 44 int HallB_2_state = 0;
hoting 0:74ea99c4db88 45 int state_2 = 0;
hoting 0:74ea99c4db88 46 int state_2_old = 0;
hoting 0:74ea99c4db88 47
hoting 0:74ea99c4db88 48 // DC motor rotation speed control
hoting 0:74ea99c4db88 49 int speed_count_1 = 0;
hoting 0:74ea99c4db88 50 float rotation_speed_1 = 0.0;
hoting 0:74ea99c4db88 51 float rotation_speed_ref_1 = 0;
hoting 0:74ea99c4db88 52 float pwm1_duty = 0.5;
hoting 0:74ea99c4db88 53 float PI_out_1 = 0.0;
hoting 0:74ea99c4db88 54 float err_1 = 0.0;
hoting 0:74ea99c4db88 55 float ierr_1 = 0.0;
hoting 0:74ea99c4db88 56 int speed_count_2 = 0;
hoting 0:74ea99c4db88 57 float rotation_speed_2 = 0.0;
ryan820909 1:13ce5b28f6dd 58 float rotation_speed_ref_2 = 150;
hoting 0:74ea99c4db88 59 float pwm2_duty = 0.5;
hoting 0:74ea99c4db88 60 float PI_out_2 = 0.0;
hoting 0:74ea99c4db88 61 float err_2 = 0.0;
hoting 0:74ea99c4db88 62 float ierr_2 = 0.0;
ryan820909 1:13ce5b28f6dd 63 float Kp = 0.4;
ryan820909 1:13ce5b28f6dd 64 float Ki = 0.1;
hoting 0:74ea99c4db88 65 int main()
hoting 0:74ea99c4db88 66 {
ryan820909 1:13ce5b28f6dd 67
ryan820909 1:13ce5b28f6dd 68
hoting 0:74ea99c4db88 69 init_PWM();
ryan820909 1:13ce5b28f6dd 70 init_CN_1();
ryan820909 1:13ce5b28f6dd 71 init_CN_2();
ryan820909 1:13ce5b28f6dd 72 init_TIMER();
hoting 0:74ea99c4db88 73 while(1) {
hoting 0:74ea99c4db88 74 }
hoting 0:74ea99c4db88 75 }
hoting 0:74ea99c4db88 76
hoting 0:74ea99c4db88 77 void init_TIMER()
hoting 0:74ea99c4db88 78 {
hoting 0:74ea99c4db88 79 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 80 }
hoting 0:74ea99c4db88 81
hoting 0:74ea99c4db88 82 void init_PWM()
hoting 0:74ea99c4db88 83 {
hoting 0:74ea99c4db88 84 servo_cmd.period_ms(20);
hoting 0:74ea99c4db88 85 servo_cmd.write(servo_duty);
hoting 0:74ea99c4db88 86
hoting 0:74ea99c4db88 87 pwm1.period_us(50);
ryan820909 1:13ce5b28f6dd 88 pwm1.write(0.5f);
hoting 0:74ea99c4db88 89 TIM1->CCER |= 0x4;
hoting 0:74ea99c4db88 90
hoting 0:74ea99c4db88 91 pwm2.period_us(50);
ryan820909 1:13ce5b28f6dd 92 pwm2.write(0.5f);
hoting 0:74ea99c4db88 93 TIM1->CCER |= 0x40;
hoting 0:74ea99c4db88 94 }
ryan820909 1:13ce5b28f6dd 95 void init_CN_1()
ryan820909 1:13ce5b28f6dd 96 {
ryan820909 1:13ce5b28f6dd 97 HallA.rise(&CN_ITR_1);
ryan820909 1:13ce5b28f6dd 98 HallA.fall(&CN_ITR_1);
ryan820909 1:13ce5b28f6dd 99 HallB.rise(&CN_ITR_1);
ryan820909 1:13ce5b28f6dd 100 HallB.fall(&CN_ITR_1);
ryan820909 1:13ce5b28f6dd 101
ryan820909 1:13ce5b28f6dd 102 }
ryan820909 1:13ce5b28f6dd 103 void init_CN_2()
hoting 0:74ea99c4db88 104 {
ryan820909 1:13ce5b28f6dd 105 HallA_2.rise(&CN_ITR_2);
ryan820909 1:13ce5b28f6dd 106 HallA_2.fall(&CN_ITR_2);
ryan820909 1:13ce5b28f6dd 107 HallB_2.rise(&CN_ITR_2);
ryan820909 1:13ce5b28f6dd 108 HallB_2.fall(&CN_ITR_2);
ryan820909 1:13ce5b28f6dd 109
ryan820909 1:13ce5b28f6dd 110 }
hoting 0:74ea99c4db88 111
ryan820909 1:13ce5b28f6dd 112
ryan820909 1:13ce5b28f6dd 113 void CN_ITR_1()
ryan820909 1:13ce5b28f6dd 114 {
ryan820909 1:13ce5b28f6dd 115 speed_count_1 = speed_count_1+1;
hoting 0:74ea99c4db88 116 }
hoting 0:74ea99c4db88 117
ryan820909 1:13ce5b28f6dd 118 void CN_ITR_2()
hoting 0:74ea99c4db88 119 {
ryan820909 1:13ce5b28f6dd 120 speed_count_2 = speed_count_2+1;
ryan820909 1:13ce5b28f6dd 121 }
hoting 0:74ea99c4db88 122
hoting 0:74ea99c4db88 123 void timer1_ITR()
hoting 0:74ea99c4db88 124 {
hoting 0:74ea99c4db88 125 // servo motor
hoting 0:74ea99c4db88 126 ///code for sevo motor///
hoting 0:74ea99c4db88 127
hoting 0:74ea99c4db88 128
hoting 0:74ea99c4db88 129
hoting 0:74ea99c4db88 130 /////////////////////////
hoting 0:74ea99c4db88 131
hoting 0:74ea99c4db88 132 if(servo_duty >= 0.113f)servo_duty = 0.113;
hoting 0:74ea99c4db88 133 else if(servo_duty <= 0.025f)servo_duty = 0.025;
hoting 0:74ea99c4db88 134 servo_cmd.write(servo_duty);
hoting 0:74ea99c4db88 135
hoting 0:74ea99c4db88 136 // motor1
hoting 0:74ea99c4db88 137 rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
ryan820909 1:13ce5b28f6dd 138 // pc.printf("%f ",rotation_speed_1);
hoting 0:74ea99c4db88 139 speed_count_1 = 0;
hoting 0:74ea99c4db88 140
hoting 0:74ea99c4db88 141 ///PI controller for motor1///
hoting 0:74ea99c4db88 142
hoting 0:74ea99c4db88 143
hoting 0:74ea99c4db88 144
hoting 0:74ea99c4db88 145 //////////////////////////////
hoting 0:74ea99c4db88 146
ryan820909 1:13ce5b28f6dd 147 /*
ryan820909 1:13ce5b28f6dd 148
hoting 0:74ea99c4db88 149 if(PI_out_1 >= 0.5f)PI_out_1 = 0.5;
hoting 0:74ea99c4db88 150 else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5;
hoting 0:74ea99c4db88 151 pwm1_duty = PI_out_1 + 0.5f;
hoting 0:74ea99c4db88 152 pwm1.write(pwm1_duty);
hoting 0:74ea99c4db88 153 TIM1->CCER |= 0x4;
ryan820909 1:13ce5b28f6dd 154 */
hoting 0:74ea99c4db88 155 //motor2
ryan820909 1:13ce5b28f6dd 156 // rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
ryan820909 1:13ce5b28f6dd 157 pc.printf("%d \n\r",speed_count_2*100/12*60/29);
hoting 0:74ea99c4db88 158 speed_count_2 = 0;
hoting 0:74ea99c4db88 159
hoting 0:74ea99c4db88 160 ///PI controller for motor2///
hoting 0:74ea99c4db88 161
hoting 0:74ea99c4db88 162
ryan820909 1:13ce5b28f6dd 163 err_2 = Kp*(rotation_speed_ref_2-rotation_speed_2);
ryan820909 1:13ce5b28f6dd 164 ierr_2 = ierr_2 +Ki*(rotation_speed_ref_2-rotation_speed_2)*0.01;
ryan820909 1:13ce5b28f6dd 165 PI_out_2 = (float)(err_2)*0.005;
hoting 0:74ea99c4db88 166
hoting 0:74ea99c4db88 167 //////////////////////////////
ryan820909 1:13ce5b28f6dd 168
hoting 0:74ea99c4db88 169 if(PI_out_2 >= 0.5f)PI_out_2 = 0.5;
hoting 0:74ea99c4db88 170 else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5;
hoting 0:74ea99c4db88 171 pwm2_duty = PI_out_2 + 0.5f;
hoting 0:74ea99c4db88 172 pwm2.write(pwm2_duty);
hoting 0:74ea99c4db88 173 TIM1->CCER |= 0x40;
ryan820909 1:13ce5b28f6dd 174
ryan820909 1:13ce5b28f6dd 175
hoting 0:74ea99c4db88 176 }