Robotic Team 1 / Mbed 2 deprecated ROS_Lab5

need to check rpm or rad/s for angular velocity

Dependencies:   mbed ros_lib_indigo

main.cpp@0:68d5289b45cc, 2018-04-20 (annotated)

Committer:
m56542321
Date:
Fri Apr 20 09:49:22 2018 +0000
Revision:
0:68d5289b45cc
controller is not finished!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
m56542321 0:68d5289b45cc 1 /* LAB DCMotor */
m56542321 0:68d5289b45cc 2 #include "mbed.h"
m56542321 0:68d5289b45cc 3 #include <ros.h>
m56542321 0:68d5289b45cc 4 #include <geometry_msgs/Vector3.h>
m56542321 0:68d5289b45cc 5 #include <geometry_msgs/Twist.h>
m56542321 0:68d5289b45cc 6
m56542321 0:68d5289b45cc 7 //****************************************************************************** Define
m56542321 0:68d5289b45cc 8 //The number will be compiled as type "double" in default
m56542321 0:68d5289b45cc 9 //Add a "f" after the number can make it compiled as type "float"
m56542321 0:68d5289b45cc 10 #define Ts 0.01f //period of timer1 (s)
m56542321 0:68d5289b45cc 11 #define PI 3.145926
m56542321 0:68d5289b45cc 12
m56542321 0:68d5289b45cc 13 // PID parameter
m56542321 0:68d5289b45cc 14 #define Kp_1 2.0f
m56542321 0:68d5289b45cc 15 #define Ki_1 0.0f
m56542321 0:68d5289b45cc 16 #define Kd_1 0.0f
m56542321 0:68d5289b45cc 17
m56542321 0:68d5289b45cc 18 #define Kp_2 3.0f
m56542321 0:68d5289b45cc 19 #define Ki_2 0.0f
m56542321 0:68d5289b45cc 20 #define Kd_2 0.0f
m56542321 0:68d5289b45cc 21
m56542321 0:68d5289b45cc 22 //****************************************************************************** End of Define
m56542321 0:68d5289b45cc 23
m56542321 0:68d5289b45cc 24 //****************************************************************************** I/O
m56542321 0:68d5289b45cc 25 //PWM
m56542321 0:68d5289b45cc 26 //Dc motor
m56542321 0:68d5289b45cc 27 PwmOut pwm1(D11);
m56542321 0:68d5289b45cc 28 PwmOut pwm1n(D7);
m56542321 0:68d5289b45cc 29 PwmOut pwm2(D8);
m56542321 0:68d5289b45cc 30 PwmOut pwm2n(A3);
m56542321 0:68d5289b45cc 31
m56542321 0:68d5289b45cc 32 //Motor1 sensor
m56542321 0:68d5289b45cc 33 InterruptIn HallA_1(A1);
m56542321 0:68d5289b45cc 34 InterruptIn HallB_1(A2);
m56542321 0:68d5289b45cc 35 //Motor2 sensor
m56542321 0:68d5289b45cc 36 InterruptIn HallA_2(D13);
m56542321 0:68d5289b45cc 37 InterruptIn HallB_2(D12);
m56542321 0:68d5289b45cc 38
m56542321 0:68d5289b45cc 39 //LED
m56542321 0:68d5289b45cc 40 DigitalOut led1(A4);
m56542321 0:68d5289b45cc 41 DigitalOut led2(A5);
m56542321 0:68d5289b45cc 42
m56542321 0:68d5289b45cc 43 //Timer Setting
m56542321 0:68d5289b45cc 44 Ticker timer;
m56542321 0:68d5289b45cc 45 //****************************************************************************** End of I/O
m56542321 0:68d5289b45cc 46
m56542321 0:68d5289b45cc 47 //****************************************************************************** Functions
m56542321 0:68d5289b45cc 48 void init_timer(void);
m56542321 0:68d5289b45cc 49 void init_CN(void);
m56542321 0:68d5289b45cc 50 void init_PWM(void);
m56542321 0:68d5289b45cc 51 void timer_interrupt(void);
m56542321 0:68d5289b45cc 52 void CN_interrupt(void);
m56542321 0:68d5289b45cc 53 //****************************************************************************** End of Functions
m56542321 0:68d5289b45cc 54
m56542321 0:68d5289b45cc 55 //****************************************************************************** Variables
m56542321 0:68d5289b45cc 56 // Servo
m56542321 0:68d5289b45cc 57 float servo_duty = 0.066; // 0.025~0.113(-90~+90) 0.069->0 degree
m56542321 0:68d5289b45cc 58
m56542321 0:68d5289b45cc 59 // motor 1
m56542321 0:68d5289b45cc 60 // decoder
m56542321 0:68d5289b45cc 61 int8_t HallA_state_1 = 0;
m56542321 0:68d5289b45cc 62 int8_t HallB_state_1 = 0;
m56542321 0:68d5289b45cc 63 int8_t motor_state_1 = 0;
m56542321 0:68d5289b45cc 64 int8_t motor_state_old_1 = 0;
m56542321 0:68d5289b45cc 65
m56542321 0:68d5289b45cc 66 // velocity
m56542321 0:68d5289b45cc 67 int count_1 = 0;
m56542321 0:68d5289b45cc 68 float speed_1 = 0.0f;
m56542321 0:68d5289b45cc 69
m56542321 0:68d5289b45cc 70 // control
m56542321 0:68d5289b45cc 71
m56542321 0:68d5289b45cc 72 // u(k) = u(k-1) + Ka*e(k) + Kb*e(k-1) + Kc*e(k-2)
m56542321 0:68d5289b45cc 73 float Ka_1 = Kp_1 + Ki_1 + Kd_1;
m56542321 0:68d5289b45cc 74 float Kb_1 = -Kp_1 - 2*Kd_1;
m56542321 0:68d5289b45cc 75 float Kc_1 = Kd_1;
m56542321 0:68d5289b45cc 76
m56542321 0:68d5289b45cc 77 float v_ref_1 = 0.0f; // r(k)
m56542321 0:68d5289b45cc 78 float v_err_1 = 0.0f; // e(k) = r(k) - v(k)
m56542321 0:68d5289b45cc 79 float v_err_old_1 = 0.0f; // e(k-1)
m56542321 0:68d5289b45cc 80 float v_err_older_1 = 0.0f; // e(k-2)
m56542321 0:68d5289b45cc 81 float ctrl_output_1 = 0.0f; // u(k)
m56542321 0:68d5289b45cc 82 float ctrl_output_old_1 = 0.0f; // u(k-1)
m56542321 0:68d5289b45cc 83 float pwm1_duty = 0.0f; // duty = u(k) + 0.5
m56542321 0:68d5289b45cc 84
m56542321 0:68d5289b45cc 85
m56542321 0:68d5289b45cc 86 // motor 2
m56542321 0:68d5289b45cc 87 // decoder
m56542321 0:68d5289b45cc 88 int8_t HallA_state_2 = 0;
m56542321 0:68d5289b45cc 89 int8_t HallB_state_2 = 0;
m56542321 0:68d5289b45cc 90 int8_t motor_state_2 = 0;
m56542321 0:68d5289b45cc 91 int8_t motor_state_old_2 = 0;
m56542321 0:68d5289b45cc 92
m56542321 0:68d5289b45cc 93 // velocity
m56542321 0:68d5289b45cc 94 int count_2 = 0;
m56542321 0:68d5289b45cc 95 float speed_2 = 0.0f;
m56542321 0:68d5289b45cc 96
m56542321 0:68d5289b45cc 97 // control
m56542321 0:68d5289b45cc 98
m56542321 0:68d5289b45cc 99 // u(k) = u(k-1) + Ka*e(k) + Kb*e(k-1) + Kc*e(k-2)
m56542321 0:68d5289b45cc 100 float Ka_2 = Kp_2 + Ki_2 + Kd_2;
m56542321 0:68d5289b45cc 101 float Kb_2 = -Kp_2 - 2*Kd_2;
m56542321 0:68d5289b45cc 102 float Kc_2 = Kd_2;
m56542321 0:68d5289b45cc 103
m56542321 0:68d5289b45cc 104 float v_ref_2 = 0.0f; // r(k)
m56542321 0:68d5289b45cc 105 float v_err_2 = 0.0f; // e(k) = r(k) - v(k)
m56542321 0:68d5289b45cc 106 float v_err_old_2 = 0.0f; // e(k-1)
m56542321 0:68d5289b45cc 107 float v_err_older_2 = 0.0f; // e(k-2)
m56542321 0:68d5289b45cc 108 float ctrl_output_2 = 0.0f; // u(k)
m56542321 0:68d5289b45cc 109 float ctrl_output_old_2 = 0.0f; // u(k-1)
m56542321 0:68d5289b45cc 110 float pwm2_duty = 0.0f; // duty = u(k) + 0.5
m56542321 0:68d5289b45cc 111
m56542321 0:68d5289b45cc 112
m56542321 0:68d5289b45cc 113
m56542321 0:68d5289b45cc 114
m56542321 0:68d5289b45cc 115 //****************************************************************************** End of Variables
m56542321 0:68d5289b45cc 116
m56542321 0:68d5289b45cc 117 //****************************************************************************** ROS
m56542321 0:68d5289b45cc 118 /*
m56542321 0:68d5289b45cc 119 * **System Structure**
m56542321 0:68d5289b45cc 120 *
m56542321 0:68d5289b45cc 121 * remote_car_node
m56542321 0:68d5289b45cc 122 * pkg : robotics
m56542321 0:68d5289b45cc 123 * pub : "/cmd_ang_vel" (Vector3)
m56542321 0:68d5289b45cc 124 * sub : "/turtlebot_teleop/cmd_vel" (Twist)
m56542321 0:68d5289b45cc 125 *
m56542321 0:68d5289b45cc 126 * turtlebot_teleop_key
m56542321 0:68d5289b45cc 127 * pkg : turtlebot_teleop
m56542321 0:68d5289b45cc 128 * pub : "/turtlebot_teleop/cmd_vel" (Twist)
m56542321 0:68d5289b45cc 129 * sub : --
m56542321 0:68d5289b45cc 130 *
m56542321 0:68d5289b45cc 131 * STM
m56542321 0:68d5289b45cc 132 * pkg :
m56542321 0:68d5289b45cc 133 * pub : "/feedback_wheel_ang_vel" (Twist)
m56542321 0:68d5289b45cc 134 * sub : "/cmd_ang_vel" (Vector3)
m56542321 0:68d5289b45cc 135 *
m56542321 0:68d5289b45cc 136 */
m56542321 0:68d5289b45cc 137
m56542321 0:68d5289b45cc 138
m56542321 0:68d5289b45cc 139 // create a ROS node handle
m56542321 0:68d5289b45cc 140 ros::NodeHandle n;
m56542321 0:68d5289b45cc 141
m56542321 0:68d5289b45cc 142 // pub a topic "/feedback_wheel_ang_vel"
m56542321 0:68d5289b45cc 143 // with type "Twist"
m56542321 0:68d5289b45cc 144 geometry_msgs::Twist wheel_ang_vel;
m56542321 0:68d5289b45cc 145 ros::Publisher pub("/feedback_wheel_ang_vel", &wheel_ang_vel);
m56542321 0:68d5289b45cc 146
m56542321 0:68d5289b45cc 147 // sub a topic "/cmd_ang_vel"
m56542321 0:68d5289b45cc 148 // with type "Vector3"
m56542321 0:68d5289b45cc 149 void messageCb(const geometry_msgs::Vector3 &cmd_received)
m56542321 0:68d5289b45cc 150 {
m56542321 0:68d5289b45cc 151 v_ref_1 = cmd_received.x;
m56542321 0:68d5289b45cc 152 v_ref_2 = cmd_received.y;
m56542321 0:68d5289b45cc 153 }
m56542321 0:68d5289b45cc 154 ros::Subscriber<geometry_msgs::Vector3> sub("/cmd_ang_vel", messageCb);
m56542321 0:68d5289b45cc 155
m56542321 0:68d5289b45cc 156
m56542321 0:68d5289b45cc 157 //****************************************************************************** End of ROS
m56542321 0:68d5289b45cc 158
m56542321 0:68d5289b45cc 159 //****************************************************************************** Main
m56542321 0:68d5289b45cc 160 int main()
m56542321 0:68d5289b45cc 161 {
m56542321 0:68d5289b45cc 162 init_PWM();
m56542321 0:68d5289b45cc 163 init_timer();
m56542321 0:68d5289b45cc 164 init_CN();
m56542321 0:68d5289b45cc 165
m56542321 0:68d5289b45cc 166 n.initNode();
m56542321 0:68d5289b45cc 167 n.subscribe(sub);
m56542321 0:68d5289b45cc 168 n.advertise(pub);
m56542321 0:68d5289b45cc 169
m56542321 0:68d5289b45cc 170 while(1)
m56542321 0:68d5289b45cc 171 {
m56542321 0:68d5289b45cc 172 wheel_ang_vel.linear.x = v_ref_1;
m56542321 0:68d5289b45cc 173 wheel_ang_vel.linear.y = speed_1;
m56542321 0:68d5289b45cc 174 wheel_ang_vel.linear.z = 0.0f;
m56542321 0:68d5289b45cc 175
m56542321 0:68d5289b45cc 176 wheel_ang_vel.angular.x = v_ref_2;
m56542321 0:68d5289b45cc 177 wheel_ang_vel.angular.y = speed_2;
m56542321 0:68d5289b45cc 178 wheel_ang_vel.angular.z = 0.0f;
m56542321 0:68d5289b45cc 179
m56542321 0:68d5289b45cc 180 pub.publish(&wheel_ang_vel);
m56542321 0:68d5289b45cc 181 n.spinOnce();
m56542321 0:68d5289b45cc 182 wait_ms(500);
m56542321 0:68d5289b45cc 183 }
m56542321 0:68d5289b45cc 184 }
m56542321 0:68d5289b45cc 185 //****************************************************************************** End of Main
m56542321 0:68d5289b45cc 186
m56542321 0:68d5289b45cc 187
m56542321 0:68d5289b45cc 188 //***************************************************************************** init_timer
m56542321 0:68d5289b45cc 189 void init_timer()
m56542321 0:68d5289b45cc 190 {
m56542321 0:68d5289b45cc 191 timer.attach_us(&timer_interrupt, 100000);//100ms interrupt period (10 Hz)
m56542321 0:68d5289b45cc 192 }
m56542321 0:68d5289b45cc 193 //****************************************************************************** End of init_timer
m56542321 0:68d5289b45cc 194
m56542321 0:68d5289b45cc 195
m56542321 0:68d5289b45cc 196 //****************************************************************************** timer_interrupt
m56542321 0:68d5289b45cc 197 void timer_interrupt()
m56542321 0:68d5289b45cc 198 {
m56542321 0:68d5289b45cc 199
m56542321 0:68d5289b45cc 200 // Motor1
m56542321 0:68d5289b45cc 201 // (period=0.01 sec, each period has 12 segments, reduction ratio 29)
m56542321 0:68d5289b45cc 202 speed_1 = (float)count_1 * 100.0f / 12.0f * 2 * PI / 29.0f; // rad/s
m56542321 0:68d5289b45cc 203 count_1 = 0;
m56542321 0:68d5289b45cc 204
m56542321 0:68d5289b45cc 205
m56542321 0:68d5289b45cc 206 // Code for PID controller //
m56542321 0:68d5289b45cc 207
m56542321 0:68d5289b45cc 208
m56542321 0:68d5289b45cc 209 v_err_1 = v_ref_1 - speed_1 ; // e(k) = r(k) - v(k)
m56542321 0:68d5289b45cc 210
m56542321 0:68d5289b45cc 211 // u(k) = u(k-1) + Ka*e(k) + Kb*e(k-1) + Kc*e(k-2)
m56542321 0:68d5289b45cc 212 ctrl_output_1 = ctrl_output_old_1 + Ka_1*v_err_1 + Kb_1*v_err_old_1 + Kc_1*v_err_older_1;
m56542321 0:68d5289b45cc 213
m56542321 0:68d5289b45cc 214 v_err_older_1 = v_err_old_1; // e(k-1)
m56542321 0:68d5289b45cc 215 v_err_old_1 = v_err_1; // e(k-2)
m56542321 0:68d5289b45cc 216 ctrl_output_old_1 = ctrl_output_1; // u(k-1)
m56542321 0:68d5289b45cc 217
m56542321 0:68d5289b45cc 218
m56542321 0:68d5289b45cc 219 // limitter of ctrl_output
m56542321 0:68d5289b45cc 220
m56542321 0:68d5289b45cc 221 if(ctrl_output_1 >= 0.5f)
m56542321 0:68d5289b45cc 222 {
m56542321 0:68d5289b45cc 223 ctrl_output_1 = 0.5f;
m56542321 0:68d5289b45cc 224 }
m56542321 0:68d5289b45cc 225 else if(ctrl_output_1 <= -0.5f)
m56542321 0:68d5289b45cc 226 {
m56542321 0:68d5289b45cc 227 ctrl_output_1 = -0.5f;
m56542321 0:68d5289b45cc 228 }
m56542321 0:68d5289b45cc 229
m56542321 0:68d5289b45cc 230 // convert crtl_output to PWM signal
m56542321 0:68d5289b45cc 231 pwm1_duty = ctrl_output_1 + 0.5f;
m56542321 0:68d5289b45cc 232 pwm1.write(pwm1_duty);
m56542321 0:68d5289b45cc 233 TIM1->CCER |= 0x4;
m56542321 0:68d5289b45cc 234
m56542321 0:68d5289b45cc 235
m56542321 0:68d5289b45cc 236 // Motor2
m56542321 0:68d5289b45cc 237 // (period=0.01 sec, each period has 12 segments, reduction ratio 29)
m56542321 0:68d5289b45cc 238 speed_2 = (float)count_2 * 100.0f / 12.0f * 2 * PI / 29.0f; // rad/s
m56542321 0:68d5289b45cc 239 count_2 = 0;
m56542321 0:68d5289b45cc 240
m56542321 0:68d5289b45cc 241
m56542321 0:68d5289b45cc 242 // Code for PID controller //
m56542321 0:68d5289b45cc 243
m56542321 0:68d5289b45cc 244
m56542321 0:68d5289b45cc 245 v_err_2 = v_ref_2 - speed_2 ; // e(k) = r(k) - v(k)
m56542321 0:68d5289b45cc 246
m56542321 0:68d5289b45cc 247 // u(k) = u(k-1) + Ka*e(k) + Kb*e(k-1) + Kc*e(k-2)
m56542321 0:68d5289b45cc 248 ctrl_output_2 = ctrl_output_old_2 + Ka_2*v_err_2 + Kb_2*v_err_old_2 + Kc_2*v_err_older_2;
m56542321 0:68d5289b45cc 249
m56542321 0:68d5289b45cc 250 v_err_older_2 = v_err_old_2; // e(k-1)
m56542321 0:68d5289b45cc 251 v_err_old_2 = v_err_2; // e(k-2)
m56542321 0:68d5289b45cc 252 ctrl_output_old_2 = ctrl_output_2; // u(k-1)
m56542321 0:68d5289b45cc 253
m56542321 0:68d5289b45cc 254
m56542321 0:68d5289b45cc 255 // limitter of ctrl_output
m56542321 0:68d5289b45cc 256
m56542321 0:68d5289b45cc 257 if(ctrl_output_2 >= 0.5f)
m56542321 0:68d5289b45cc 258 {
m56542321 0:68d5289b45cc 259 ctrl_output_2 = 0.5f;
m56542321 0:68d5289b45cc 260 }
m56542321 0:68d5289b45cc 261 else if(ctrl_output_2 <= -0.5f)
m56542321 0:68d5289b45cc 262 {
m56542321 0:68d5289b45cc 263 ctrl_output_2 = -0.5f;
m56542321 0:68d5289b45cc 264 }
m56542321 0:68d5289b45cc 265
m56542321 0:68d5289b45cc 266 // convert crtl_output to PWM signal
m56542321 0:68d5289b45cc 267 pwm2_duty = ctrl_output_2 + 0.5f;
m56542321 0:68d5289b45cc 268 pwm2.write(pwm2_duty);
m56542321 0:68d5289b45cc 269 TIM1->CCER |= 0x40;
m56542321 0:68d5289b45cc 270
m56542321 0:68d5289b45cc 271 }
m56542321 0:68d5289b45cc 272 //****************************************************************************** End of timer_interrupt
m56542321 0:68d5289b45cc 273
m56542321 0:68d5289b45cc 274 //****************************************************************************** CN_interrupt
m56542321 0:68d5289b45cc 275 void CN_interrupt_1()
m56542321 0:68d5289b45cc 276 {
m56542321 0:68d5289b45cc 277 // Motor1
m56542321 0:68d5289b45cc 278 // Read the current status of hall sensor
m56542321 0:68d5289b45cc 279 HallA_state_1 = HallA_1.read();
m56542321 0:68d5289b45cc 280 HallB_state_1 = HallB_1.read();
m56542321 0:68d5289b45cc 281
m56542321 0:68d5289b45cc 282 ///code for state determination///
m56542321 0:68d5289b45cc 283 if(HallA_state_1 == 0)
m56542321 0:68d5289b45cc 284 {
m56542321 0:68d5289b45cc 285 if(HallB_state_1 == 0)
m56542321 0:68d5289b45cc 286 {// (0,0) -> 1
m56542321 0:68d5289b45cc 287 motor_state_1 = 1;
m56542321 0:68d5289b45cc 288 }
m56542321 0:68d5289b45cc 289 else
m56542321 0:68d5289b45cc 290 {// (0,1) -> 2
m56542321 0:68d5289b45cc 291 motor_state_1 = 2;
m56542321 0:68d5289b45cc 292 }
m56542321 0:68d5289b45cc 293 }
m56542321 0:68d5289b45cc 294 else
m56542321 0:68d5289b45cc 295 {
m56542321 0:68d5289b45cc 296 if(HallB_state_1 == 0)
m56542321 0:68d5289b45cc 297 {// (1,0) -> 4
m56542321 0:68d5289b45cc 298 motor_state_1 = 4;
m56542321 0:68d5289b45cc 299 }
m56542321 0:68d5289b45cc 300 else
m56542321 0:68d5289b45cc 301 {// (1,1) -> 3
m56542321 0:68d5289b45cc 302 motor_state_1 = 3;
m56542321 0:68d5289b45cc 303 }
m56542321 0:68d5289b45cc 304 }
m56542321 0:68d5289b45cc 305
m56542321 0:68d5289b45cc 306
m56542321 0:68d5289b45cc 307 // determind direction and count hall sensor changed //
m56542321 0:68d5289b45cc 308 switch(motor_state_1)
m56542321 0:68d5289b45cc 309 {
m56542321 0:68d5289b45cc 310 case 1:
m56542321 0:68d5289b45cc 311 if(motor_state_old_1 == 4)
m56542321 0:68d5289b45cc 312 count_1--;
m56542321 0:68d5289b45cc 313 else if(motor_state_old_1 == 2)
m56542321 0:68d5289b45cc 314 count_1++;
m56542321 0:68d5289b45cc 315 break;
m56542321 0:68d5289b45cc 316 case 2:
m56542321 0:68d5289b45cc 317 if(motor_state_old_1 == 1)
m56542321 0:68d5289b45cc 318 count_1--;
m56542321 0:68d5289b45cc 319 else if(motor_state_old_1 == 3)
m56542321 0:68d5289b45cc 320 count_1++;
m56542321 0:68d5289b45cc 321 break;
m56542321 0:68d5289b45cc 322 case 3:
m56542321 0:68d5289b45cc 323 if(motor_state_old_1== 2)
m56542321 0:68d5289b45cc 324 count_1--;
m56542321 0:68d5289b45cc 325 else if(motor_state_old_1 == 4)
m56542321 0:68d5289b45cc 326 count_1++;
m56542321 0:68d5289b45cc 327 break;
m56542321 0:68d5289b45cc 328 case 4:
m56542321 0:68d5289b45cc 329 if(motor_state_old_1 == 3)
m56542321 0:68d5289b45cc 330 count_1--;
m56542321 0:68d5289b45cc 331 else if(motor_state_old_1 == 1)
m56542321 0:68d5289b45cc 332 count_1++;
m56542321 0:68d5289b45cc 333 break;
m56542321 0:68d5289b45cc 334 }
m56542321 0:68d5289b45cc 335
m56542321 0:68d5289b45cc 336 motor_state_old_1 = motor_state_1;
m56542321 0:68d5289b45cc 337
m56542321 0:68d5289b45cc 338 }
m56542321 0:68d5289b45cc 339
m56542321 0:68d5289b45cc 340 void CN_interrupt_2()
m56542321 0:68d5289b45cc 341 {
m56542321 0:68d5289b45cc 342 // Motor2
m56542321 0:68d5289b45cc 343 // Read the current status of hall sensor
m56542321 0:68d5289b45cc 344 HallA_state_2 = HallA_2.read();
m56542321 0:68d5289b45cc 345 HallB_state_2 = HallB_2.read();
m56542321 0:68d5289b45cc 346
m56542321 0:68d5289b45cc 347 ///code for state determination///
m56542321 0:68d5289b45cc 348 if(HallA_state_2 == 0)
m56542321 0:68d5289b45cc 349 {
m56542321 0:68d5289b45cc 350 if(HallB_state_2 == 0)
m56542321 0:68d5289b45cc 351 {// (0,0) -> 1
m56542321 0:68d5289b45cc 352 motor_state_2 = 1;
m56542321 0:68d5289b45cc 353 }
m56542321 0:68d5289b45cc 354 else
m56542321 0:68d5289b45cc 355 {// (0,1) -> 2
m56542321 0:68d5289b45cc 356 motor_state_2 = 2;
m56542321 0:68d5289b45cc 357 }
m56542321 0:68d5289b45cc 358 }
m56542321 0:68d5289b45cc 359 else
m56542321 0:68d5289b45cc 360 {
m56542321 0:68d5289b45cc 361 if(HallB_state_2 == 0)
m56542321 0:68d5289b45cc 362 {// (1,0) -> 4
m56542321 0:68d5289b45cc 363 motor_state_2 = 4;
m56542321 0:68d5289b45cc 364 }
m56542321 0:68d5289b45cc 365 else
m56542321 0:68d5289b45cc 366 {// (1,1) -> 3
m56542321 0:68d5289b45cc 367 motor_state_2 = 3;
m56542321 0:68d5289b45cc 368 }
m56542321 0:68d5289b45cc 369 }
m56542321 0:68d5289b45cc 370
m56542321 0:68d5289b45cc 371 // determind direction and count hall sensor changed //
m56542321 0:68d5289b45cc 372 switch(motor_state_2)
m56542321 0:68d5289b45cc 373 {
m56542321 0:68d5289b45cc 374 case 1:
m56542321 0:68d5289b45cc 375 if(motor_state_old_2 == 4)
m56542321 0:68d5289b45cc 376 count_2++;
m56542321 0:68d5289b45cc 377 else if(motor_state_old_2 == 2)
m56542321 0:68d5289b45cc 378 count_2--;
m56542321 0:68d5289b45cc 379 break;
m56542321 0:68d5289b45cc 380 case 2:
m56542321 0:68d5289b45cc 381 if(motor_state_old_2 == 1)
m56542321 0:68d5289b45cc 382 count_2++;
m56542321 0:68d5289b45cc 383 else if(motor_state_old_2 == 3)
m56542321 0:68d5289b45cc 384 count_2--;
m56542321 0:68d5289b45cc 385 break;
m56542321 0:68d5289b45cc 386 case 3:
m56542321 0:68d5289b45cc 387 if(motor_state_old_2== 2)
m56542321 0:68d5289b45cc 388 count_2++;
m56542321 0:68d5289b45cc 389 else if(motor_state_old_2 == 4)
m56542321 0:68d5289b45cc 390 count_2--;
m56542321 0:68d5289b45cc 391 break;
m56542321 0:68d5289b45cc 392 case 4:
m56542321 0:68d5289b45cc 393 if(motor_state_old_2 == 3)
m56542321 0:68d5289b45cc 394 count_2++;
m56542321 0:68d5289b45cc 395 else if(motor_state_old_2 == 1)
m56542321 0:68d5289b45cc 396 count_2--;
m56542321 0:68d5289b45cc 397 break;
m56542321 0:68d5289b45cc 398 }
m56542321 0:68d5289b45cc 399
m56542321 0:68d5289b45cc 400 motor_state_old_2 = motor_state_2;
m56542321 0:68d5289b45cc 401
m56542321 0:68d5289b45cc 402 }
m56542321 0:68d5289b45cc 403 //****************************************************************************** End of CN_interrupt
m56542321 0:68d5289b45cc 404
m56542321 0:68d5289b45cc 405
m56542321 0:68d5289b45cc 406
m56542321 0:68d5289b45cc 407 //****************************************************************************** init_PWM
m56542321 0:68d5289b45cc 408 void init_PWM()
m56542321 0:68d5289b45cc 409 {
m56542321 0:68d5289b45cc 410 // motor 1
m56542321 0:68d5289b45cc 411 pwm1.period_us(50);
m56542321 0:68d5289b45cc 412 pwm1.pulsewidth_us(25);
m56542321 0:68d5289b45cc 413
m56542321 0:68d5289b45cc 414 TIM1->CCER |= 0x4;
m56542321 0:68d5289b45cc 415
m56542321 0:68d5289b45cc 416 // motor 2
m56542321 0:68d5289b45cc 417 pwm2.period_us(50);
m56542321 0:68d5289b45cc 418 pwm2.pulsewidth_us(25);
m56542321 0:68d5289b45cc 419
m56542321 0:68d5289b45cc 420 TIM1->CCER |= 0x40;
m56542321 0:68d5289b45cc 421 }
m56542321 0:68d5289b45cc 422 //****************************************************************************** End of init_PWM
m56542321 0:68d5289b45cc 423
m56542321 0:68d5289b45cc 424 //****************************************************************************** init_CN
m56542321 0:68d5289b45cc 425 void init_CN()
m56542321 0:68d5289b45cc 426 {
m56542321 0:68d5289b45cc 427 // Motor1
m56542321 0:68d5289b45cc 428 HallA_1.rise(&CN_interrupt_1);
m56542321 0:68d5289b45cc 429 HallA_1.fall(&CN_interrupt_1);
m56542321 0:68d5289b45cc 430 HallB_1.rise(&CN_interrupt_1);
m56542321 0:68d5289b45cc 431 HallB_1.fall(&CN_interrupt_1);
m56542321 0:68d5289b45cc 432
m56542321 0:68d5289b45cc 433 HallA_state_1 = HallA_1.read();
m56542321 0:68d5289b45cc 434 HallB_state_1 = HallB_1.read();
m56542321 0:68d5289b45cc 435
m56542321 0:68d5289b45cc 436 // Motor2
m56542321 0:68d5289b45cc 437 HallA_2.rise(&CN_interrupt_2);
m56542321 0:68d5289b45cc 438 HallA_2.fall(&CN_interrupt_2);
m56542321 0:68d5289b45cc 439 HallB_2.rise(&CN_interrupt_2);
m56542321 0:68d5289b45cc 440 HallB_2.fall(&CN_interrupt_2);
m56542321 0:68d5289b45cc 441
m56542321 0:68d5289b45cc 442 HallA_state_2 = HallA_2.read();
m56542321 0:68d5289b45cc 443 HallB_state_2 = HallB_2.read();
m56542321 0:68d5289b45cc 444
m56542321 0:68d5289b45cc 445 }
m56542321 0:68d5289b45cc 446 //****************************************************************************** End of init_CN