Robotic Team 1 / Mbed 2 deprecated ROS_Remote_Car

LAB5

Dependencies:   mbed ros_lib_indigo

RemoteCar.cpp@0:2d8724cb4e57, 2018-04-19 (annotated)

Committer:
MechanicalThomas
Date:
Thu Apr 19 15:02:33 2018 +0000
Revision:
0:2d8724cb4e57
lab5;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MechanicalThomas 0:2d8724cb4e57 1 /* LAB DCMotor */
MechanicalThomas 0:2d8724cb4e57 2 #include "mbed.h"
MechanicalThomas 0:2d8724cb4e57 3 #include <ros.h>
MechanicalThomas 0:2d8724cb4e57 4 #include <geometry_msgs/Vector3.h>
MechanicalThomas 0:2d8724cb4e57 5 #include <geometry_msgs/Twist.h>
MechanicalThomas 0:2d8724cb4e57 6
MechanicalThomas 0:2d8724cb4e57 7
MechanicalThomas 0:2d8724cb4e57 8 //****************************************************************************** Define
MechanicalThomas 0:2d8724cb4e57 9 //The number will be compiled as type "double" in default
MechanicalThomas 0:2d8724cb4e57 10 //Add a "f" after the number can make it compiled as type "float"
MechanicalThomas 0:2d8724cb4e57 11 #define Ts 0.01f //period of timer1 (s)
MechanicalThomas 0:2d8724cb4e57 12 #define PI 3.1415926
MechanicalThomas 0:2d8724cb4e57 13 //****************************************************************************** End of Define
MechanicalThomas 0:2d8724cb4e57 14
MechanicalThomas 0:2d8724cb4e57 15 //****************************************************************************** I/O
MechanicalThomas 0:2d8724cb4e57 16 //PWM
MechanicalThomas 0:2d8724cb4e57 17 //Dc motor
MechanicalThomas 0:2d8724cb4e57 18 PwmOut pwm1(D7);
MechanicalThomas 0:2d8724cb4e57 19 PwmOut pwm1n(D11);
MechanicalThomas 0:2d8724cb4e57 20 PwmOut pwm2(D8);
MechanicalThomas 0:2d8724cb4e57 21 PwmOut pwm2n(A3);
MechanicalThomas 0:2d8724cb4e57 22
MechanicalThomas 0:2d8724cb4e57 23 //Motor1 sensor
MechanicalThomas 0:2d8724cb4e57 24 InterruptIn HallA_1(A1);
MechanicalThomas 0:2d8724cb4e57 25 InterruptIn HallB_1(A2);
MechanicalThomas 0:2d8724cb4e57 26 //Motor2 sensor
MechanicalThomas 0:2d8724cb4e57 27 InterruptIn HallA_2(D13);
MechanicalThomas 0:2d8724cb4e57 28 InterruptIn HallB_2(D12);
MechanicalThomas 0:2d8724cb4e57 29
MechanicalThomas 0:2d8724cb4e57 30 //LED
MechanicalThomas 0:2d8724cb4e57 31 DigitalOut led1(A4);
MechanicalThomas 0:2d8724cb4e57 32 DigitalOut led2(A5);
MechanicalThomas 0:2d8724cb4e57 33
MechanicalThomas 0:2d8724cb4e57 34 //Timer Setting
MechanicalThomas 0:2d8724cb4e57 35 Ticker timer;
MechanicalThomas 0:2d8724cb4e57 36 //****************************************************************************** End of I/O
MechanicalThomas 0:2d8724cb4e57 37
MechanicalThomas 0:2d8724cb4e57 38 //****************************************************************************** Functions
MechanicalThomas 0:2d8724cb4e57 39 void init_timer(void);
MechanicalThomas 0:2d8724cb4e57 40 void init_CN(void);
MechanicalThomas 0:2d8724cb4e57 41 void init_PWM(void);
MechanicalThomas 0:2d8724cb4e57 42 void timer_interrupt(void);
MechanicalThomas 0:2d8724cb4e57 43 void CN_interrupt(void);
MechanicalThomas 0:2d8724cb4e57 44 //****************************************************************************** End of Functions
MechanicalThomas 0:2d8724cb4e57 45
MechanicalThomas 0:2d8724cb4e57 46 //****************************************************************************** Variables
MechanicalThomas 0:2d8724cb4e57 47 // Servo
MechanicalThomas 0:2d8724cb4e57 48 float servo_duty = 0.066; // 0.025~0.113(-90~+90) 0.069->0 degree
MechanicalThomas 0:2d8724cb4e57 49
MechanicalThomas 0:2d8724cb4e57 50 // motor 1
MechanicalThomas 0:2d8724cb4e57 51 int8_t HallA_state_1 = 0;
MechanicalThomas 0:2d8724cb4e57 52 int8_t HallB_state_1 = 0;
MechanicalThomas 0:2d8724cb4e57 53 int8_t motor_state_1 = 0;
MechanicalThomas 0:2d8724cb4e57 54 int8_t motor_state_old_1 = 0;
MechanicalThomas 0:2d8724cb4e57 55 int count_1 = 0;
MechanicalThomas 0:2d8724cb4e57 56 float speed_1 = 0.0f;
MechanicalThomas 0:2d8724cb4e57 57 float v_ref_1 = 80.0f;
MechanicalThomas 0:2d8724cb4e57 58 float v_err_1 = 0.0f; // v_err_old_1 = v_err_1 ; v_err_1 = v_ref_1 - speed_1 ;
MechanicalThomas 0:2d8724cb4e57 59 float v_ierr_1 = 0.0f; //integral error : v_ierr_1 = v_err_old_1 + v_err_1;
MechanicalThomas 0:2d8724cb4e57 60 float ctrl_output_1 = 0.0f;
MechanicalThomas 0:2d8724cb4e57 61 float pwm1_duty = 0.0f;
MechanicalThomas 0:2d8724cb4e57 62
MechanicalThomas 0:2d8724cb4e57 63 float Kp_1 = 10; //need to be tested
MechanicalThomas 0:2d8724cb4e57 64 float Ki_1 = 0.05; //need to be tested
MechanicalThomas 0:2d8724cb4e57 65 //float ctrl_output_old_1 = 0.0f;
MechanicalThomas 0:2d8724cb4e57 66
MechanicalThomas 0:2d8724cb4e57 67
MechanicalThomas 0:2d8724cb4e57 68 //motor 2
MechanicalThomas 0:2d8724cb4e57 69 int8_t HallA_state_2 = 0;
MechanicalThomas 0:2d8724cb4e57 70 int8_t HallB_state_2 = 0;
MechanicalThomas 0:2d8724cb4e57 71 int8_t motor_state_2 = 0;
MechanicalThomas 0:2d8724cb4e57 72 int8_t motor_state_old_2 = 0;
MechanicalThomas 0:2d8724cb4e57 73 int count_2 = 0;
MechanicalThomas 0:2d8724cb4e57 74 float speed_2 = 0.0f;
MechanicalThomas 0:2d8724cb4e57 75 float v_ref_2 = 150.0f;
MechanicalThomas 0:2d8724cb4e57 76 float v_err_2 = 0.0f;
MechanicalThomas 0:2d8724cb4e57 77 float v_ierr_2 = 0.0f;
MechanicalThomas 0:2d8724cb4e57 78 float ctrl_output_2 = 0.0f;
MechanicalThomas 0:2d8724cb4e57 79 float pwm2_duty = 0.0f;
MechanicalThomas 0:2d8724cb4e57 80
MechanicalThomas 0:2d8724cb4e57 81
MechanicalThomas 0:2d8724cb4e57 82 float Kp_2 = 10;
MechanicalThomas 0:2d8724cb4e57 83 float Ki_2 = 0.05;
MechanicalThomas 0:2d8724cb4e57 84 //float ctrl_output_old_2 = 0.0f;
MechanicalThomas 0:2d8724cb4e57 85
MechanicalThomas 0:2d8724cb4e57 86
MechanicalThomas 0:2d8724cb4e57 87 //****************************************************************************** End of Variables
MechanicalThomas 0:2d8724cb4e57 88 ros::NodeHandle nh;
MechanicalThomas 0:2d8724cb4e57 89
MechanicalThomas 0:2d8724cb4e57 90 geometry_msgs::Twist vel_msg;
MechanicalThomas 0:2d8724cb4e57 91 ros::Publisher p("feedback_wheel_angularVel", &vel_msg);
MechanicalThomas 0:2d8724cb4e57 92
MechanicalThomas 0:2d8724cb4e57 93
MechanicalThomas 0:2d8724cb4e57 94 void messageCb(const geometry_msgs::Vector3& msg)
MechanicalThomas 0:2d8724cb4e57 95 {
MechanicalThomas 0:2d8724cb4e57 96 //receive velocity cmd from PC to motor
MechanicalThomas 0:2d8724cb4e57 97
MechanicalThomas 0:2d8724cb4e57 98
MechanicalThomas 0:2d8724cb4e57 99 }
MechanicalThomas 0:2d8724cb4e57 100
MechanicalThomas 0:2d8724cb4e57 101 ros:: Subscriber<geometry_msgs::Vector3> s("cmd_wheel_angularVel", messageCb);
MechanicalThomas 0:2d8724cb4e57 102 //****************************************************************************** Main
MechanicalThomas 0:2d8724cb4e57 103 int main()
MechanicalThomas 0:2d8724cb4e57 104 {
MechanicalThomas 0:2d8724cb4e57 105 init_PWM();
MechanicalThomas 0:2d8724cb4e57 106 init_timer();
MechanicalThomas 0:2d8724cb4e57 107 init_CN();
MechanicalThomas 0:2d8724cb4e57 108 nh.initNode();
MechanicalThomas 0:2d8724cb4e57 109 nh.subscribe(s);
MechanicalThomas 0:2d8724cb4e57 110 nh.advertise(p);
MechanicalThomas 0:2d8724cb4e57 111
MechanicalThomas 0:2d8724cb4e57 112
MechanicalThomas 0:2d8724cb4e57 113 while(1)
MechanicalThomas 0:2d8724cb4e57 114 {
MechanicalThomas 0:2d8724cb4e57 115 vel_msg.linear.x = v_ref_1;
MechanicalThomas 0:2d8724cb4e57 116 vel_msg.linear.y = speed_1;
MechanicalThomas 0:2d8724cb4e57 117 vel_msg.linear.z = 0.0f;
MechanicalThomas 0:2d8724cb4e57 118
MechanicalThomas 0:2d8724cb4e57 119 vel_msg.angular.x = v_ref_2;
MechanicalThomas 0:2d8724cb4e57 120 vel_msg.angular.y = speed_2;
MechanicalThomas 0:2d8724cb4e57 121 vel_msg.angular.z = 0.0f;
MechanicalThomas 0:2d8724cb4e57 122
MechanicalThomas 0:2d8724cb4e57 123 p.publish(&vel_msg);
MechanicalThomas 0:2d8724cb4e57 124 nh.spinOnce();
MechanicalThomas 0:2d8724cb4e57 125 wait_ms(1000);
MechanicalThomas 0:2d8724cb4e57 126
MechanicalThomas 0:2d8724cb4e57 127 }
MechanicalThomas 0:2d8724cb4e57 128 }
MechanicalThomas 0:2d8724cb4e57 129 //****************************************************************************** End of Main
MechanicalThomas 0:2d8724cb4e57 130
MechanicalThomas 0:2d8724cb4e57 131 //****************************************************************************** timer_interrupt
MechanicalThomas 0:2d8724cb4e57 132 void timer_interrupt()
MechanicalThomas 0:2d8724cb4e57 133 {
MechanicalThomas 0:2d8724cb4e57 134 // Motor1
MechanicalThomas 0:2d8724cb4e57 135 speed_1 = (float)count_1 * 100.0f / 12.0f * 2.0f * PI/ 29.0f; //(rad/s) of output wheel (period=0.01 sec, each period has 12 segments, reduction ratio 29)
MechanicalThomas 0:2d8724cb4e57 136 count_1 = 0;
MechanicalThomas 0:2d8724cb4e57 137 // Code for PI controller //
MechanicalThomas 0:2d8724cb4e57 138
MechanicalThomas 0:2d8724cb4e57 139
MechanicalThomas 0:2d8724cb4e57 140 v_err_1 = v_ref_1 - speed_1 ;
MechanicalThomas 0:2d8724cb4e57 141
MechanicalThomas 0:2d8724cb4e57 142 ctrl_output_1 = (Kp_1)*(v_err_1) + (Ki_1)*(v_ierr_1);
MechanicalThomas 0:2d8724cb4e57 143
MechanicalThomas 0:2d8724cb4e57 144 v_ierr_1 = (v_ierr_1) + (v_err_1)*Ts ;
MechanicalThomas 0:2d8724cb4e57 145
MechanicalThomas 0:2d8724cb4e57 146
MechanicalThomas 0:2d8724cb4e57 147
MechanicalThomas 0:2d8724cb4e57 148 ///////////////////////////
MechanicalThomas 0:2d8724cb4e57 149
MechanicalThomas 0:2d8724cb4e57 150 if(ctrl_output_1 >= 0.5f)ctrl_output_1 = 0.5f;
MechanicalThomas 0:2d8724cb4e57 151 else if(ctrl_output_1 <= -0.5f)ctrl_output_1 = -0.5f;
MechanicalThomas 0:2d8724cb4e57 152 pwm1_duty = ctrl_output_1 + 0.5f;
MechanicalThomas 0:2d8724cb4e57 153 pwm1.write(pwm1_duty);
MechanicalThomas 0:2d8724cb4e57 154 TIM1->CCER |= 0x4;
MechanicalThomas 0:2d8724cb4e57 155
MechanicalThomas 0:2d8724cb4e57 156
MechanicalThomas 0:2d8724cb4e57 157
MechanicalThomas 0:2d8724cb4e57 158 // Motor2
MechanicalThomas 0:2d8724cb4e57 159 speed_2 = (float)count_2 * 100.0f / 12.0f * 2.0f * PI/ 29.0f; //(rad/s)
MechanicalThomas 0:2d8724cb4e57 160 count_2 = 0;
MechanicalThomas 0:2d8724cb4e57 161 // Code for PI controller //
MechanicalThomas 0:2d8724cb4e57 162
MechanicalThomas 0:2d8724cb4e57 163 v_err_2 = v_ref_2 - speed_2 ;
MechanicalThomas 0:2d8724cb4e57 164
MechanicalThomas 0:2d8724cb4e57 165 ctrl_output_2 = (Kp_2)*(v_err_2) + (Ki_2)*(v_ierr_2);
MechanicalThomas 0:2d8724cb4e57 166
MechanicalThomas 0:2d8724cb4e57 167 v_ierr_2 = (v_ierr_2) + (v_err_2)*Ts ;
MechanicalThomas 0:2d8724cb4e57 168
MechanicalThomas 0:2d8724cb4e57 169
MechanicalThomas 0:2d8724cb4e57 170 ///////////////////////////
MechanicalThomas 0:2d8724cb4e57 171
MechanicalThomas 0:2d8724cb4e57 172 if(ctrl_output_2 >= 0.5f)ctrl_output_2 = 0.5f;
MechanicalThomas 0:2d8724cb4e57 173 else if(ctrl_output_2 <= -0.5f)ctrl_output_2 = -0.5f;
MechanicalThomas 0:2d8724cb4e57 174 pwm2_duty = ctrl_output_2 + 0.5f;
MechanicalThomas 0:2d8724cb4e57 175 pwm2.write(pwm2_duty);
MechanicalThomas 0:2d8724cb4e57 176 TIM1->CCER |= 0x40;
MechanicalThomas 0:2d8724cb4e57 177
MechanicalThomas 0:2d8724cb4e57 178 }
MechanicalThomas 0:2d8724cb4e57 179 //****************************************************************************** End of timer_interrupt
MechanicalThomas 0:2d8724cb4e57 180
MechanicalThomas 0:2d8724cb4e57 181 //****************************************************************************** CN_interrupt
MechanicalThomas 0:2d8724cb4e57 182 void CN_interrupt()
MechanicalThomas 0:2d8724cb4e57 183 {
MechanicalThomas 0:2d8724cb4e57 184 // Motor1
MechanicalThomas 0:2d8724cb4e57 185 // Read the current status of hall sensor
MechanicalThomas 0:2d8724cb4e57 186 HallA_state_1 = HallA_1.read();
MechanicalThomas 0:2d8724cb4e57 187 HallB_state_1 = HallB_1.read();
MechanicalThomas 0:2d8724cb4e57 188
MechanicalThomas 0:2d8724cb4e57 189 ///code for state determination///
MechanicalThomas 0:2d8724cb4e57 190 if(HallA_state_1==0)
MechanicalThomas 0:2d8724cb4e57 191 {
MechanicalThomas 0:2d8724cb4e57 192 if(HallB_state_1==0)
MechanicalThomas 0:2d8724cb4e57 193 {
MechanicalThomas 0:2d8724cb4e57 194 motor_state_1 =1;
MechanicalThomas 0:2d8724cb4e57 195 }
MechanicalThomas 0:2d8724cb4e57 196 else
MechanicalThomas 0:2d8724cb4e57 197 {
MechanicalThomas 0:2d8724cb4e57 198 motor_state_1 =2;
MechanicalThomas 0:2d8724cb4e57 199 }
MechanicalThomas 0:2d8724cb4e57 200 }
MechanicalThomas 0:2d8724cb4e57 201 else
MechanicalThomas 0:2d8724cb4e57 202 {
MechanicalThomas 0:2d8724cb4e57 203 if(HallB_state_1==0)
MechanicalThomas 0:2d8724cb4e57 204 {
MechanicalThomas 0:2d8724cb4e57 205 motor_state_1 = 3;
MechanicalThomas 0:2d8724cb4e57 206 }
MechanicalThomas 0:2d8724cb4e57 207 else
MechanicalThomas 0:2d8724cb4e57 208 {
MechanicalThomas 0:2d8724cb4e57 209 motor_state_1 = 4;
MechanicalThomas 0:2d8724cb4e57 210 }
MechanicalThomas 0:2d8724cb4e57 211 }
MechanicalThomas 0:2d8724cb4e57 212
MechanicalThomas 0:2d8724cb4e57 213
MechanicalThomas 0:2d8724cb4e57 214 //////////////////////////////////
MechanicalThomas 0:2d8724cb4e57 215 switch(motor_state_1)
MechanicalThomas 0:2d8724cb4e57 216 {
MechanicalThomas 0:2d8724cb4e57 217 case 1:
MechanicalThomas 0:2d8724cb4e57 218 if(motor_state_old_1 == 4)
MechanicalThomas 0:2d8724cb4e57 219 count_1--;
MechanicalThomas 0:2d8724cb4e57 220 else if(motor_state_old_1 == 2)
MechanicalThomas 0:2d8724cb4e57 221 count_1++;
MechanicalThomas 0:2d8724cb4e57 222 break;
MechanicalThomas 0:2d8724cb4e57 223 case 2:
MechanicalThomas 0:2d8724cb4e57 224 if(motor_state_old_1 == 1)
MechanicalThomas 0:2d8724cb4e57 225 count_1--;
MechanicalThomas 0:2d8724cb4e57 226 else if(motor_state_old_1 == 3)
MechanicalThomas 0:2d8724cb4e57 227 count_1++;
MechanicalThomas 0:2d8724cb4e57 228 break;
MechanicalThomas 0:2d8724cb4e57 229 case 3:
MechanicalThomas 0:2d8724cb4e57 230 if(motor_state_old_1== 2)
MechanicalThomas 0:2d8724cb4e57 231 count_1--;
MechanicalThomas 0:2d8724cb4e57 232 else if(motor_state_old_1 == 4)
MechanicalThomas 0:2d8724cb4e57 233 count_1++;
MechanicalThomas 0:2d8724cb4e57 234 break;
MechanicalThomas 0:2d8724cb4e57 235 case 4:
MechanicalThomas 0:2d8724cb4e57 236 if(motor_state_old_1 == 3)
MechanicalThomas 0:2d8724cb4e57 237 count_1--;
MechanicalThomas 0:2d8724cb4e57 238 else if(motor_state_old_1 == 1)
MechanicalThomas 0:2d8724cb4e57 239 count_1++;
MechanicalThomas 0:2d8724cb4e57 240 break;
MechanicalThomas 0:2d8724cb4e57 241 }
MechanicalThomas 0:2d8724cb4e57 242 motor_state_old_1 = motor_state_1;
MechanicalThomas 0:2d8724cb4e57 243 //Forward
MechanicalThomas 0:2d8724cb4e57 244 //v1Count +1
MechanicalThomas 0:2d8724cb4e57 245 //Inverse
MechanicalThomas 0:2d8724cb4e57 246 //v1Count -1
MechanicalThomas 0:2d8724cb4e57 247
MechanicalThomas 0:2d8724cb4e57 248 // Motor2
MechanicalThomas 0:2d8724cb4e57 249 // Read the current status of hall sensor
MechanicalThomas 0:2d8724cb4e57 250 HallA_state_2 = HallA_2.read();
MechanicalThomas 0:2d8724cb4e57 251 HallB_state_2 = HallB_2.read();
MechanicalThomas 0:2d8724cb4e57 252
MechanicalThomas 0:2d8724cb4e57 253 ///code for state determination///
MechanicalThomas 0:2d8724cb4e57 254 if(HallA_state_2==0)
MechanicalThomas 0:2d8724cb4e57 255 {
MechanicalThomas 0:2d8724cb4e57 256 if(HallB_state_2==0)
MechanicalThomas 0:2d8724cb4e57 257 {
MechanicalThomas 0:2d8724cb4e57 258 motor_state_2 =1;
MechanicalThomas 0:2d8724cb4e57 259 }
MechanicalThomas 0:2d8724cb4e57 260 else
MechanicalThomas 0:2d8724cb4e57 261 {
MechanicalThomas 0:2d8724cb4e57 262 motor_state_2 =2;
MechanicalThomas 0:2d8724cb4e57 263 }
MechanicalThomas 0:2d8724cb4e57 264 }
MechanicalThomas 0:2d8724cb4e57 265 else
MechanicalThomas 0:2d8724cb4e57 266 {
MechanicalThomas 0:2d8724cb4e57 267 if(HallB_state_2==0)
MechanicalThomas 0:2d8724cb4e57 268 {
MechanicalThomas 0:2d8724cb4e57 269 motor_state_2 = 3;
MechanicalThomas 0:2d8724cb4e57 270 }
MechanicalThomas 0:2d8724cb4e57 271 else
MechanicalThomas 0:2d8724cb4e57 272 {
MechanicalThomas 0:2d8724cb4e57 273 motor_state_2 = 4;
MechanicalThomas 0:2d8724cb4e57 274 }
MechanicalThomas 0:2d8724cb4e57 275 }
MechanicalThomas 0:2d8724cb4e57 276
MechanicalThomas 0:2d8724cb4e57 277 //////////////////////////////////
MechanicalThomas 0:2d8724cb4e57 278 switch(motor_state_2)
MechanicalThomas 0:2d8724cb4e57 279 {
MechanicalThomas 0:2d8724cb4e57 280 case 1:
MechanicalThomas 0:2d8724cb4e57 281 if(motor_state_old_2 == 4)
MechanicalThomas 0:2d8724cb4e57 282 count_2--;
MechanicalThomas 0:2d8724cb4e57 283 else if(motor_state_old_2 == 2)
MechanicalThomas 0:2d8724cb4e57 284 count_2++;
MechanicalThomas 0:2d8724cb4e57 285 break;
MechanicalThomas 0:2d8724cb4e57 286 case 2:
MechanicalThomas 0:2d8724cb4e57 287 if(motor_state_old_2 == 1)
MechanicalThomas 0:2d8724cb4e57 288 count_1--;
MechanicalThomas 0:2d8724cb4e57 289 else if(motor_state_old_2 == 3)
MechanicalThomas 0:2d8724cb4e57 290 count_2++;
MechanicalThomas 0:2d8724cb4e57 291 break;
MechanicalThomas 0:2d8724cb4e57 292 case 3:
MechanicalThomas 0:2d8724cb4e57 293 if(motor_state_old_2== 2)
MechanicalThomas 0:2d8724cb4e57 294 count_2--;
MechanicalThomas 0:2d8724cb4e57 295 else if(motor_state_old_2 == 4)
MechanicalThomas 0:2d8724cb4e57 296 count_2++;
MechanicalThomas 0:2d8724cb4e57 297 break;
MechanicalThomas 0:2d8724cb4e57 298 case 4:
MechanicalThomas 0:2d8724cb4e57 299 if(motor_state_old_2 == 3)
MechanicalThomas 0:2d8724cb4e57 300 count_2--;
MechanicalThomas 0:2d8724cb4e57 301 else if(motor_state_old_2 == 1)
MechanicalThomas 0:2d8724cb4e57 302 count_2++;
MechanicalThomas 0:2d8724cb4e57 303 break;
MechanicalThomas 0:2d8724cb4e57 304 }
MechanicalThomas 0:2d8724cb4e57 305 motor_state_old_2 = motor_state_2;
MechanicalThomas 0:2d8724cb4e57 306
MechanicalThomas 0:2d8724cb4e57 307 //Forward
MechanicalThomas 0:2d8724cb4e57 308 //v2Count +1
MechanicalThomas 0:2d8724cb4e57 309 //Inverse
MechanicalThomas 0:2d8724cb4e57 310 //v2Count -1
MechanicalThomas 0:2d8724cb4e57 311 }
MechanicalThomas 0:2d8724cb4e57 312 //****************************************************************************** End of CN_interrupt
MechanicalThomas 0:2d8724cb4e57 313
MechanicalThomas 0:2d8724cb4e57 314 //****************************************************************************** init_timer
MechanicalThomas 0:2d8724cb4e57 315 void init_timer()
MechanicalThomas 0:2d8724cb4e57 316 {
MechanicalThomas 0:2d8724cb4e57 317 timer.attach_us(&timer_interrupt, 10000);//10ms interrupt period (100 Hz)
MechanicalThomas 0:2d8724cb4e57 318 }
MechanicalThomas 0:2d8724cb4e57 319 //****************************************************************************** End of init_timer
MechanicalThomas 0:2d8724cb4e57 320
MechanicalThomas 0:2d8724cb4e57 321 //****************************************************************************** init_PWM
MechanicalThomas 0:2d8724cb4e57 322 void init_PWM()
MechanicalThomas 0:2d8724cb4e57 323 {
MechanicalThomas 0:2d8724cb4e57 324 pwm1.period_us(50);
MechanicalThomas 0:2d8724cb4e57 325 pwm1.write(0.5);
MechanicalThomas 0:2d8724cb4e57 326 TIM1->CCER |= 0x4;
MechanicalThomas 0:2d8724cb4e57 327
MechanicalThomas 0:2d8724cb4e57 328 pwm2.period_us(50);
MechanicalThomas 0:2d8724cb4e57 329 pwm2.write(0.5);
MechanicalThomas 0:2d8724cb4e57 330 TIM1->CCER |= 0x40;
MechanicalThomas 0:2d8724cb4e57 331 }
MechanicalThomas 0:2d8724cb4e57 332 //****************************************************************************** End of init_PWM
MechanicalThomas 0:2d8724cb4e57 333
MechanicalThomas 0:2d8724cb4e57 334 //****************************************************************************** init_CN
MechanicalThomas 0:2d8724cb4e57 335 void init_CN()
MechanicalThomas 0:2d8724cb4e57 336 {
MechanicalThomas 0:2d8724cb4e57 337 // Motor1
MechanicalThomas 0:2d8724cb4e57 338 HallA_1.rise(&CN_interrupt);
MechanicalThomas 0:2d8724cb4e57 339 HallA_1.fall(&CN_interrupt);
MechanicalThomas 0:2d8724cb4e57 340 HallB_1.rise(&CN_interrupt);
MechanicalThomas 0:2d8724cb4e57 341 HallB_1.fall(&CN_interrupt);
MechanicalThomas 0:2d8724cb4e57 342
MechanicalThomas 0:2d8724cb4e57 343 HallA_state_1 = HallA_1.read();
MechanicalThomas 0:2d8724cb4e57 344 HallB_state_1 = HallB_1.read();
MechanicalThomas 0:2d8724cb4e57 345
MechanicalThomas 0:2d8724cb4e57 346 // Motor2
MechanicalThomas 0:2d8724cb4e57 347 HallA_2.rise(&CN_interrupt);
MechanicalThomas 0:2d8724cb4e57 348 HallA_2.fall(&CN_interrupt);
MechanicalThomas 0:2d8724cb4e57 349 HallB_2.rise(&CN_interrupt);
MechanicalThomas 0:2d8724cb4e57 350 HallB_2.fall(&CN_interrupt);
MechanicalThomas 0:2d8724cb4e57 351
MechanicalThomas 0:2d8724cb4e57 352 HallA_state_2 = HallA_2.read();
MechanicalThomas 0:2d8724cb4e57 353 HallB_state_2 = HallB_2.read();
MechanicalThomas 0:2d8724cb4e57 354 }
MechanicalThomas 0:2d8724cb4e57 355 //****************************************************************************** End of init_CN