Robotics ^___^ / Mbed 2 deprecated Lab5

^_______________________^

Dependencies:   mbed ros_lib_indigo

main.cpp@0:06d4e6dd8f7b, 2017-03-30 (annotated)

Committer:
Andy_Lee
Date:
Thu Mar 30 13:54:03 2017 +0000
Revision:
0:06d4e6dd8f7b
hope you guys  finish a.s.a.p^________^

Who changed what in which revision?

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