lab5
Dependencies: mbed ros_lib_indigo
main.cpp@0:0846fa975a83, 2017-03-28 (annotated)
- Committer:
- tea5062001
- Date:
- Tue Mar 28 13:14:57 2017 +0000
- Revision:
- 0:0846fa975a83
lab5
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tea5062001 | 0:0846fa975a83 | 1 | #include "mbed.h" |
tea5062001 | 0:0846fa975a83 | 2 | #include <ros.h> |
tea5062001 | 0:0846fa975a83 | 3 | #include <geometry_msgs/Twist.h> |
tea5062001 | 0:0846fa975a83 | 4 | #include <geometry_msgs/Vector3.h> |
tea5062001 | 0:0846fa975a83 | 5 | |
tea5062001 | 0:0846fa975a83 | 6 | //The number will be compiled as type "double" in default |
tea5062001 | 0:0846fa975a83 | 7 | //Add a "f" after the number can make it compiled as type "float" |
tea5062001 | 0:0846fa975a83 | 8 | #define Ts 0.01f //period of timer1 (s) |
tea5062001 | 0:0846fa975a83 | 9 | #define Kp 0.006f |
tea5062001 | 0:0846fa975a83 | 10 | #define Ki 0.02f |
tea5062001 | 0:0846fa975a83 | 11 | |
tea5062001 | 0:0846fa975a83 | 12 | Ticker timer1; |
tea5062001 | 0:0846fa975a83 | 13 | // servo motor |
tea5062001 | 0:0846fa975a83 | 14 | PwmOut servo_cmd(A0); |
tea5062001 | 0:0846fa975a83 | 15 | // DC motor |
tea5062001 | 0:0846fa975a83 | 16 | PwmOut pwm1(D7); |
tea5062001 | 0:0846fa975a83 | 17 | PwmOut pwm1n(D11); |
tea5062001 | 0:0846fa975a83 | 18 | PwmOut pwm2(D8); |
tea5062001 | 0:0846fa975a83 | 19 | PwmOut pwm2n(A3); |
tea5062001 | 0:0846fa975a83 | 20 | |
tea5062001 | 0:0846fa975a83 | 21 | // Motor1 sensor |
tea5062001 | 0:0846fa975a83 | 22 | InterruptIn HallA(A1); |
tea5062001 | 0:0846fa975a83 | 23 | InterruptIn HallB(A2); |
tea5062001 | 0:0846fa975a83 | 24 | // Motor2 sensor |
tea5062001 | 0:0846fa975a83 | 25 | InterruptIn HallA_2(D13); |
tea5062001 | 0:0846fa975a83 | 26 | InterruptIn HallB_2(D12); |
tea5062001 | 0:0846fa975a83 | 27 | |
tea5062001 | 0:0846fa975a83 | 28 | // 函式宣告 |
tea5062001 | 0:0846fa975a83 | 29 | void init_IO(); |
tea5062001 | 0:0846fa975a83 | 30 | void init_TIMER(); |
tea5062001 | 0:0846fa975a83 | 31 | void timer1_ITR(); |
tea5062001 | 0:0846fa975a83 | 32 | void init_CN(); |
tea5062001 | 0:0846fa975a83 | 33 | void CN_ITR(); |
tea5062001 | 0:0846fa975a83 | 34 | void init_PWM(); |
tea5062001 | 0:0846fa975a83 | 35 | |
tea5062001 | 0:0846fa975a83 | 36 | // servo motor |
tea5062001 | 0:0846fa975a83 | 37 | float servo_duty = 0.025; // 0.069 +(0.088/180)*angle, -90<angle<90 |
tea5062001 | 0:0846fa975a83 | 38 | // 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113 |
tea5062001 | 0:0846fa975a83 | 39 | int angle = 0; |
tea5062001 | 0:0846fa975a83 | 40 | int counter; |
tea5062001 | 0:0846fa975a83 | 41 | |
tea5062001 | 0:0846fa975a83 | 42 | |
tea5062001 | 0:0846fa975a83 | 43 | // Hall sensor |
tea5062001 | 0:0846fa975a83 | 44 | int HallA_1_state = 0; |
tea5062001 | 0:0846fa975a83 | 45 | int HallB_1_state = 0; |
tea5062001 | 0:0846fa975a83 | 46 | int state_1 = 0; |
tea5062001 | 0:0846fa975a83 | 47 | int state_1_old = 0; |
tea5062001 | 0:0846fa975a83 | 48 | int HallA_2_state = 0; |
tea5062001 | 0:0846fa975a83 | 49 | int HallB_2_state = 0; |
tea5062001 | 0:0846fa975a83 | 50 | int state_2 = 0; |
tea5062001 | 0:0846fa975a83 | 51 | int state_2_old = 0; |
tea5062001 | 0:0846fa975a83 | 52 | |
tea5062001 | 0:0846fa975a83 | 53 | int c = 0; |
tea5062001 | 0:0846fa975a83 | 54 | int d = 0; |
tea5062001 | 0:0846fa975a83 | 55 | |
tea5062001 | 0:0846fa975a83 | 56 | // DC motor rotation speed control |
tea5062001 | 0:0846fa975a83 | 57 | int speed_count_1 = 0; |
tea5062001 | 0:0846fa975a83 | 58 | float rotation_speed_1 = 0.0; |
tea5062001 | 0:0846fa975a83 | 59 | float rotation_speed_ref_1 = 0; |
tea5062001 | 0:0846fa975a83 | 60 | float pwm1_duty = 0.5; |
tea5062001 | 0:0846fa975a83 | 61 | float PI_out_1 = 0.0; |
tea5062001 | 0:0846fa975a83 | 62 | float err_1 = 0.0; |
tea5062001 | 0:0846fa975a83 | 63 | float ierr_1 = 0.0; |
tea5062001 | 0:0846fa975a83 | 64 | int speed_count_2 = 0; |
tea5062001 | 0:0846fa975a83 | 65 | float rotation_speed_2 = 0.0; |
tea5062001 | 0:0846fa975a83 | 66 | float rotation_speed_ref_2 = 0; |
tea5062001 | 0:0846fa975a83 | 67 | float pwm2_duty = 0.5; |
tea5062001 | 0:0846fa975a83 | 68 | float PI_out_2 = 0.0; |
tea5062001 | 0:0846fa975a83 | 69 | float err_2 = 0.0; |
tea5062001 | 0:0846fa975a83 | 70 | float ierr_2 = 0.0; |
tea5062001 | 0:0846fa975a83 | 71 | |
tea5062001 | 0:0846fa975a83 | 72 | //rosserial |
tea5062001 | 0:0846fa975a83 | 73 | ros::NodeHandle nh; |
tea5062001 | 0:0846fa975a83 | 74 | |
tea5062001 | 0:0846fa975a83 | 75 | geometry_msgs::Twist vel_msg; |
tea5062001 | 0:0846fa975a83 | 76 | ros::Publisher p("feedback_wheel_angularVel", &vel_msg); |
tea5062001 | 0:0846fa975a83 | 77 | |
tea5062001 | 0:0846fa975a83 | 78 | void messageCallback(const geometry_msgs::Vector3 &msg_receive) |
tea5062001 | 0:0846fa975a83 | 79 | { |
tea5062001 | 0:0846fa975a83 | 80 | rotation_speed_ref_1 = -msg_receive.x; |
tea5062001 | 0:0846fa975a83 | 81 | rotation_speed_ref_2 = msg_receive.y; |
tea5062001 | 0:0846fa975a83 | 82 | } |
tea5062001 | 0:0846fa975a83 | 83 | |
tea5062001 | 0:0846fa975a83 | 84 | ros::Subscriber<geometry_msgs::Vector3> s("cmd_wheel_angularVel",messageCallback); |
tea5062001 | 0:0846fa975a83 | 85 | |
tea5062001 | 0:0846fa975a83 | 86 | int main() |
tea5062001 | 0:0846fa975a83 | 87 | { |
tea5062001 | 0:0846fa975a83 | 88 | init_TIMER(); |
tea5062001 | 0:0846fa975a83 | 89 | init_PWM(); |
tea5062001 | 0:0846fa975a83 | 90 | init_CN(); |
tea5062001 | 0:0846fa975a83 | 91 | |
tea5062001 | 0:0846fa975a83 | 92 | nh.initNode(); |
tea5062001 | 0:0846fa975a83 | 93 | nh.subscribe(s); |
tea5062001 | 0:0846fa975a83 | 94 | nh.advertise(p); |
tea5062001 | 0:0846fa975a83 | 95 | |
tea5062001 | 0:0846fa975a83 | 96 | while(1) |
tea5062001 | 0:0846fa975a83 | 97 | { |
tea5062001 | 0:0846fa975a83 | 98 | vel_msg.linear.x = rotation_speed_ref_1; |
tea5062001 | 0:0846fa975a83 | 99 | vel_msg.linear.y = rotation_speed_1; |
tea5062001 | 0:0846fa975a83 | 100 | vel_msg.linear.z = 0; |
tea5062001 | 0:0846fa975a83 | 101 | |
tea5062001 | 0:0846fa975a83 | 102 | vel_msg.angular.x = rotation_speed_ref_2; |
tea5062001 | 0:0846fa975a83 | 103 | vel_msg.angular.y = rotation_speed_2; |
tea5062001 | 0:0846fa975a83 | 104 | vel_msg.angular.z = 0; |
tea5062001 | 0:0846fa975a83 | 105 | |
tea5062001 | 0:0846fa975a83 | 106 | p.publish(&vel_msg); |
tea5062001 | 0:0846fa975a83 | 107 | |
tea5062001 | 0:0846fa975a83 | 108 | nh.spinOnce(); |
tea5062001 | 0:0846fa975a83 | 109 | wait_ms(20); |
tea5062001 | 0:0846fa975a83 | 110 | } |
tea5062001 | 0:0846fa975a83 | 111 | } |
tea5062001 | 0:0846fa975a83 | 112 | |
tea5062001 | 0:0846fa975a83 | 113 | void init_TIMER() |
tea5062001 | 0:0846fa975a83 | 114 | { |
tea5062001 | 0:0846fa975a83 | 115 | timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (10000 micro-seconds) |
tea5062001 | 0:0846fa975a83 | 116 | } |
tea5062001 | 0:0846fa975a83 | 117 | |
tea5062001 | 0:0846fa975a83 | 118 | void init_PWM() |
tea5062001 | 0:0846fa975a83 | 119 | { |
tea5062001 | 0:0846fa975a83 | 120 | servo_cmd.period_ms(20); |
tea5062001 | 0:0846fa975a83 | 121 | servo_cmd.write(servo_duty); |
tea5062001 | 0:0846fa975a83 | 122 | |
tea5062001 | 0:0846fa975a83 | 123 | pwm1.period_us(50); |
tea5062001 | 0:0846fa975a83 | 124 | pwm1.write(0.5); |
tea5062001 | 0:0846fa975a83 | 125 | TIM1->CCER |= 0x4; |
tea5062001 | 0:0846fa975a83 | 126 | |
tea5062001 | 0:0846fa975a83 | 127 | pwm2.period_us(50); |
tea5062001 | 0:0846fa975a83 | 128 | pwm2.write(0.5); |
tea5062001 | 0:0846fa975a83 | 129 | TIM1->CCER |= 0x40; |
tea5062001 | 0:0846fa975a83 | 130 | } |
tea5062001 | 0:0846fa975a83 | 131 | void init_CN() |
tea5062001 | 0:0846fa975a83 | 132 | { |
tea5062001 | 0:0846fa975a83 | 133 | HallA.rise(&CN_ITR); |
tea5062001 | 0:0846fa975a83 | 134 | HallA.fall(&CN_ITR); |
tea5062001 | 0:0846fa975a83 | 135 | HallB.rise(&CN_ITR); |
tea5062001 | 0:0846fa975a83 | 136 | HallB.fall(&CN_ITR); |
tea5062001 | 0:0846fa975a83 | 137 | |
tea5062001 | 0:0846fa975a83 | 138 | HallA_2.rise(&CN_ITR); |
tea5062001 | 0:0846fa975a83 | 139 | HallA_2.fall(&CN_ITR); |
tea5062001 | 0:0846fa975a83 | 140 | HallB_2.rise(&CN_ITR); |
tea5062001 | 0:0846fa975a83 | 141 | HallB_2.fall(&CN_ITR); |
tea5062001 | 0:0846fa975a83 | 142 | } |
tea5062001 | 0:0846fa975a83 | 143 | |
tea5062001 | 0:0846fa975a83 | 144 | void CN_ITR() |
tea5062001 | 0:0846fa975a83 | 145 | { |
tea5062001 | 0:0846fa975a83 | 146 | // motor1 |
tea5062001 | 0:0846fa975a83 | 147 | HallA_1_state = HallA.read(); |
tea5062001 | 0:0846fa975a83 | 148 | HallB_1_state = HallB.read(); |
tea5062001 | 0:0846fa975a83 | 149 | |
tea5062001 | 0:0846fa975a83 | 150 | ///code for state determination/// |
tea5062001 | 0:0846fa975a83 | 151 | |
tea5062001 | 0:0846fa975a83 | 152 | state_1 = 10*HallA_1_state + HallB_1_state; //state = AB (ex:A=1,B=0, state_1 = 10) |
tea5062001 | 0:0846fa975a83 | 153 | |
tea5062001 | 0:0846fa975a83 | 154 | if(state_1_old != state_1) |
tea5062001 | 0:0846fa975a83 | 155 | { |
tea5062001 | 0:0846fa975a83 | 156 | if((state_1_old/10) == (state_1_old%10)) |
tea5062001 | 0:0846fa975a83 | 157 | { |
tea5062001 | 0:0846fa975a83 | 158 | if((state_1%10) != (state_1_old%10)) |
tea5062001 | 0:0846fa975a83 | 159 | { |
tea5062001 | 0:0846fa975a83 | 160 | speed_count_1++; |
tea5062001 | 0:0846fa975a83 | 161 | } |
tea5062001 | 0:0846fa975a83 | 162 | else if((state_1/10) != (state_1_old/10)) |
tea5062001 | 0:0846fa975a83 | 163 | { |
tea5062001 | 0:0846fa975a83 | 164 | speed_count_1--; |
tea5062001 | 0:0846fa975a83 | 165 | } |
tea5062001 | 0:0846fa975a83 | 166 | } |
tea5062001 | 0:0846fa975a83 | 167 | else if((state_1_old/10) != (state_1_old%10)) |
tea5062001 | 0:0846fa975a83 | 168 | { |
tea5062001 | 0:0846fa975a83 | 169 | if((state_1%10) != (state_1_old%10)) |
tea5062001 | 0:0846fa975a83 | 170 | { |
tea5062001 | 0:0846fa975a83 | 171 | speed_count_1--; |
tea5062001 | 0:0846fa975a83 | 172 | } |
tea5062001 | 0:0846fa975a83 | 173 | else if((state_1/10) != (state_1_old/10)) |
tea5062001 | 0:0846fa975a83 | 174 | { |
tea5062001 | 0:0846fa975a83 | 175 | speed_count_1++; |
tea5062001 | 0:0846fa975a83 | 176 | } |
tea5062001 | 0:0846fa975a83 | 177 | } |
tea5062001 | 0:0846fa975a83 | 178 | |
tea5062001 | 0:0846fa975a83 | 179 | state_1_old = state_1; |
tea5062001 | 0:0846fa975a83 | 180 | } |
tea5062001 | 0:0846fa975a83 | 181 | |
tea5062001 | 0:0846fa975a83 | 182 | |
tea5062001 | 0:0846fa975a83 | 183 | ////////////////////////////////// |
tea5062001 | 0:0846fa975a83 | 184 | |
tea5062001 | 0:0846fa975a83 | 185 | //forward : speed_count_1 + 1 |
tea5062001 | 0:0846fa975a83 | 186 | //backward : speed_count_1 - 1 |
tea5062001 | 0:0846fa975a83 | 187 | |
tea5062001 | 0:0846fa975a83 | 188 | |
tea5062001 | 0:0846fa975a83 | 189 | // motor2 |
tea5062001 | 0:0846fa975a83 | 190 | HallA_2_state = HallA_2.read(); |
tea5062001 | 0:0846fa975a83 | 191 | HallB_2_state = HallB_2.read(); |
tea5062001 | 0:0846fa975a83 | 192 | |
tea5062001 | 0:0846fa975a83 | 193 | ///code for state determination/// |
tea5062001 | 0:0846fa975a83 | 194 | |
tea5062001 | 0:0846fa975a83 | 195 | state_2 = 10*HallA_2_state + HallB_2_state; //state = AB (ex:A=1,B=0, state_1 = 10) |
tea5062001 | 0:0846fa975a83 | 196 | |
tea5062001 | 0:0846fa975a83 | 197 | if(state_2_old != state_2) |
tea5062001 | 0:0846fa975a83 | 198 | { |
tea5062001 | 0:0846fa975a83 | 199 | if((state_2_old/10) == (state_2_old%10)) |
tea5062001 | 0:0846fa975a83 | 200 | { |
tea5062001 | 0:0846fa975a83 | 201 | if((state_2%10) != (state_2_old%10)) |
tea5062001 | 0:0846fa975a83 | 202 | { |
tea5062001 | 0:0846fa975a83 | 203 | speed_count_2++; |
tea5062001 | 0:0846fa975a83 | 204 | } |
tea5062001 | 0:0846fa975a83 | 205 | else if((state_2/10) != (state_2_old/10)) |
tea5062001 | 0:0846fa975a83 | 206 | { |
tea5062001 | 0:0846fa975a83 | 207 | speed_count_2--; |
tea5062001 | 0:0846fa975a83 | 208 | } |
tea5062001 | 0:0846fa975a83 | 209 | } |
tea5062001 | 0:0846fa975a83 | 210 | else if((state_2_old/10) != (state_2_old%10)) |
tea5062001 | 0:0846fa975a83 | 211 | { |
tea5062001 | 0:0846fa975a83 | 212 | if((state_2%10) != (state_2_old%10)) |
tea5062001 | 0:0846fa975a83 | 213 | { |
tea5062001 | 0:0846fa975a83 | 214 | speed_count_2--; |
tea5062001 | 0:0846fa975a83 | 215 | } |
tea5062001 | 0:0846fa975a83 | 216 | else if((state_2/10) != (state_2_old/10)) |
tea5062001 | 0:0846fa975a83 | 217 | { |
tea5062001 | 0:0846fa975a83 | 218 | speed_count_2++; |
tea5062001 | 0:0846fa975a83 | 219 | } |
tea5062001 | 0:0846fa975a83 | 220 | } |
tea5062001 | 0:0846fa975a83 | 221 | |
tea5062001 | 0:0846fa975a83 | 222 | state_2_old = state_2; |
tea5062001 | 0:0846fa975a83 | 223 | } |
tea5062001 | 0:0846fa975a83 | 224 | |
tea5062001 | 0:0846fa975a83 | 225 | |
tea5062001 | 0:0846fa975a83 | 226 | |
tea5062001 | 0:0846fa975a83 | 227 | ////////////////////////////////// |
tea5062001 | 0:0846fa975a83 | 228 | |
tea5062001 | 0:0846fa975a83 | 229 | //forward : speed_count_2 + 1 |
tea5062001 | 0:0846fa975a83 | 230 | //backward : speed_count_2 - 1 |
tea5062001 | 0:0846fa975a83 | 231 | } |
tea5062001 | 0:0846fa975a83 | 232 | |
tea5062001 | 0:0846fa975a83 | 233 | void timer1_ITR() |
tea5062001 | 0:0846fa975a83 | 234 | { |
tea5062001 | 0:0846fa975a83 | 235 | // servo motor |
tea5062001 | 0:0846fa975a83 | 236 | ///code for sevo motor/// |
tea5062001 | 0:0846fa975a83 | 237 | |
tea5062001 | 0:0846fa975a83 | 238 | counter = counter + 1; |
tea5062001 | 0:0846fa975a83 | 239 | |
tea5062001 | 0:0846fa975a83 | 240 | if(counter == 100) |
tea5062001 | 0:0846fa975a83 | 241 | { |
tea5062001 | 0:0846fa975a83 | 242 | servo_duty = 0.069; |
tea5062001 | 0:0846fa975a83 | 243 | } |
tea5062001 | 0:0846fa975a83 | 244 | if(counter == 200) |
tea5062001 | 0:0846fa975a83 | 245 | { |
tea5062001 | 0:0846fa975a83 | 246 | servo_duty = 0.0763; |
tea5062001 | 0:0846fa975a83 | 247 | } |
tea5062001 | 0:0846fa975a83 | 248 | if(counter == 300) |
tea5062001 | 0:0846fa975a83 | 249 | { |
tea5062001 | 0:0846fa975a83 | 250 | servo_duty = 0.0837; |
tea5062001 | 0:0846fa975a83 | 251 | } |
tea5062001 | 0:0846fa975a83 | 252 | if(counter == 400) |
tea5062001 | 0:0846fa975a83 | 253 | { |
tea5062001 | 0:0846fa975a83 | 254 | servo_duty = 0.091; |
tea5062001 | 0:0846fa975a83 | 255 | } |
tea5062001 | 0:0846fa975a83 | 256 | if(counter == 500) |
tea5062001 | 0:0846fa975a83 | 257 | { |
tea5062001 | 0:0846fa975a83 | 258 | servo_duty = 0.0983; |
tea5062001 | 0:0846fa975a83 | 259 | } |
tea5062001 | 0:0846fa975a83 | 260 | if(counter == 600) |
tea5062001 | 0:0846fa975a83 | 261 | { |
tea5062001 | 0:0846fa975a83 | 262 | servo_duty = 0.106; |
tea5062001 | 0:0846fa975a83 | 263 | } |
tea5062001 | 0:0846fa975a83 | 264 | |
tea5062001 | 0:0846fa975a83 | 265 | if(counter == 700) |
tea5062001 | 0:0846fa975a83 | 266 | { |
tea5062001 | 0:0846fa975a83 | 267 | servo_duty = 0.113; |
tea5062001 | 0:0846fa975a83 | 268 | } |
tea5062001 | 0:0846fa975a83 | 269 | if(counter > 700) |
tea5062001 | 0:0846fa975a83 | 270 | { |
tea5062001 | 0:0846fa975a83 | 271 | counter=0; |
tea5062001 | 0:0846fa975a83 | 272 | } |
tea5062001 | 0:0846fa975a83 | 273 | |
tea5062001 | 0:0846fa975a83 | 274 | servo_cmd.write(servo_duty); |
tea5062001 | 0:0846fa975a83 | 275 | |
tea5062001 | 0:0846fa975a83 | 276 | |
tea5062001 | 0:0846fa975a83 | 277 | ///////////////////////// |
tea5062001 | 0:0846fa975a83 | 278 | |
tea5062001 | 0:0846fa975a83 | 279 | if(servo_duty >= 0.113f)servo_duty = 0.113; |
tea5062001 | 0:0846fa975a83 | 280 | else if(servo_duty <= 0.025f)servo_duty = 0.025; |
tea5062001 | 0:0846fa975a83 | 281 | servo_cmd.write(servo_duty); |
tea5062001 | 0:0846fa975a83 | 282 | |
tea5062001 | 0:0846fa975a83 | 283 | // motor1 |
tea5062001 | 0:0846fa975a83 | 284 | rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f ; //unit: rpm |
tea5062001 | 0:0846fa975a83 | 285 | speed_count_1 = 0; |
tea5062001 | 0:0846fa975a83 | 286 | |
tea5062001 | 0:0846fa975a83 | 287 | ///PI controller for motor1/// |
tea5062001 | 0:0846fa975a83 | 288 | |
tea5062001 | 0:0846fa975a83 | 289 | err_1 = rotation_speed_ref_1 - rotation_speed_1; |
tea5062001 | 0:0846fa975a83 | 290 | ierr_1 = ierr_1 + err_1*Ts; |
tea5062001 | 0:0846fa975a83 | 291 | PI_out_1 = Kp*err_1 + Ki*ierr_1; |
tea5062001 | 0:0846fa975a83 | 292 | |
tea5062001 | 0:0846fa975a83 | 293 | ////////////////////////////// |
tea5062001 | 0:0846fa975a83 | 294 | |
tea5062001 | 0:0846fa975a83 | 295 | if(PI_out_1 >= 0.5f)PI_out_1 = 0.5; |
tea5062001 | 0:0846fa975a83 | 296 | else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5; |
tea5062001 | 0:0846fa975a83 | 297 | pwm1_duty = PI_out_1 + 0.5f; |
tea5062001 | 0:0846fa975a83 | 298 | pwm1.write(PI_out_1 + 0.5f); |
tea5062001 | 0:0846fa975a83 | 299 | TIM1->CCER |= 0x4; |
tea5062001 | 0:0846fa975a83 | 300 | |
tea5062001 | 0:0846fa975a83 | 301 | //motor2 |
tea5062001 | 0:0846fa975a83 | 302 | rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm |
tea5062001 | 0:0846fa975a83 | 303 | speed_count_2 = 0; |
tea5062001 | 0:0846fa975a83 | 304 | |
tea5062001 | 0:0846fa975a83 | 305 | ///PI controller for motor2/// |
tea5062001 | 0:0846fa975a83 | 306 | |
tea5062001 | 0:0846fa975a83 | 307 | err_2 = rotation_speed_ref_2 - rotation_speed_2; |
tea5062001 | 0:0846fa975a83 | 308 | ierr_2 = ierr_2 + err_2*Ts; |
tea5062001 | 0:0846fa975a83 | 309 | PI_out_2 = Kp*err_2 + Ki*ierr_2; |
tea5062001 | 0:0846fa975a83 | 310 | |
tea5062001 | 0:0846fa975a83 | 311 | ////////////////////////////// |
tea5062001 | 0:0846fa975a83 | 312 | |
tea5062001 | 0:0846fa975a83 | 313 | if(PI_out_2 >= 0.5f)PI_out_2 = 0.5; |
tea5062001 | 0:0846fa975a83 | 314 | else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5; |
tea5062001 | 0:0846fa975a83 | 315 | pwm2_duty = PI_out_2 + 0.5f; |
tea5062001 | 0:0846fa975a83 | 316 | pwm2.write(PI_out_2 + 0.5f); |
tea5062001 | 0:0846fa975a83 | 317 | TIM1->CCER |= 0x40; |
tea5062001 | 0:0846fa975a83 | 318 | } |