NTHU機器人學 Team3 / Mbed 2 deprecated Bluetooth

trytrykang

Dependencies:   mbed ros_lib_indigo

Fork of ROS_remote_car by NTHU機器人學 Team3

main.cpp@0:5e356103dcc7, 2018-04-12 (annotated)

Committer:
farmookong
Date:
Thu Apr 12 09:59:57 2018 +0000
Revision:
0:5e356103dcc7
Child:
1:d24c3384bc59
LAB4SERVOOK

Who changed what in which revision?

UserRevisionLine numberNew contents of line
farmookong 0:5e356103dcc7 1 /* LAB DCMotor */
farmookong 0:5e356103dcc7 2 #include "mbed.h"
farmookong 0:5e356103dcc7 3
farmookong 0:5e356103dcc7 4 //****************************************************************************** Define
farmookong 0:5e356103dcc7 5 //The number will be compiled as type "double" in default
farmookong 0:5e356103dcc7 6 //Add a "f" after the number can make it compiled as type "float"
farmookong 0:5e356103dcc7 7 #define Ts 0.01f //period of timer1 (s)
farmookong 0:5e356103dcc7 8 #define Servo_Period 20
farmookong 0:5e356103dcc7 9 //****************************************************************************** End of Define
farmookong 0:5e356103dcc7 10
farmookong 0:5e356103dcc7 11 //****************************************************************************** I/O
farmookong 0:5e356103dcc7 12 //PWM
farmookong 0:5e356103dcc7 13 Serial pc(USBTX, USBRX); // tx, rx
farmookong 0:5e356103dcc7 14
farmookong 0:5e356103dcc7 15 //Dc motor
farmookong 0:5e356103dcc7 16 PwmOut pwm1(D7);
farmookong 0:5e356103dcc7 17 PwmOut pwm1n(D11);
farmookong 0:5e356103dcc7 18 PwmOut pwm2(D8);
farmookong 0:5e356103dcc7 19 PwmOut pwm2n(A3);
farmookong 0:5e356103dcc7 20 PwmOut servo(A0);
farmookong 0:5e356103dcc7 21 //Motor1 sensor
farmookong 0:5e356103dcc7 22 InterruptIn HallA_1(A1);
farmookong 0:5e356103dcc7 23 InterruptIn HallB_1(A2);
farmookong 0:5e356103dcc7 24 //Motor2 sensor
farmookong 0:5e356103dcc7 25 InterruptIn HallA_2(D13);
farmookong 0:5e356103dcc7 26 InterruptIn HallB_2(D12);
farmookong 0:5e356103dcc7 27
farmookong 0:5e356103dcc7 28 //LED
farmookong 0:5e356103dcc7 29 DigitalOut led1(A4);
farmookong 0:5e356103dcc7 30 DigitalOut led2(A5);
farmookong 0:5e356103dcc7 31
farmookong 0:5e356103dcc7 32 //Timer Setting
farmookong 0:5e356103dcc7 33 Ticker timer;
farmookong 0:5e356103dcc7 34 //****************************************************************************** End of I/O
farmookong 0:5e356103dcc7 35
farmookong 0:5e356103dcc7 36 //****************************************************************************** Functions
farmookong 0:5e356103dcc7 37 void init_timer(void);
farmookong 0:5e356103dcc7 38 void init_CN(void);
farmookong 0:5e356103dcc7 39 void init_PWM(void);
farmookong 0:5e356103dcc7 40 void timer_interrupt(void);
farmookong 0:5e356103dcc7 41 void CN_interrupt(void);
farmookong 0:5e356103dcc7 42 //****************************************************************************** End of Functions
farmookong 0:5e356103dcc7 43
farmookong 0:5e356103dcc7 44 //****************************************************************************** Variables
farmookong 0:5e356103dcc7 45 // Servo
farmookong 0:5e356103dcc7 46 float servo_duty = 0.03; // 0.025~0.113(-90~+90) 0.069->0 degree
farmookong 0:5e356103dcc7 47 // motor 1
farmookong 0:5e356103dcc7 48 int8_t HallA_state_1 = 0;
farmookong 0:5e356103dcc7 49 int8_t HallB_state_1 = 0;
farmookong 0:5e356103dcc7 50 int8_t motor_state_1 = 0;
farmookong 0:5e356103dcc7 51 int8_t motor_state_old_1 = 0;
farmookong 0:5e356103dcc7 52 int count_1 = 0;
farmookong 0:5e356103dcc7 53 float speed_1 = 0.0f;
farmookong 0:5e356103dcc7 54 float v_ref_1 = -80.0f;
farmookong 0:5e356103dcc7 55 float v_err_1 = 0.0f;
farmookong 0:5e356103dcc7 56 float v_ierr_1 = 0.0f;
farmookong 0:5e356103dcc7 57 float ctrl_output_1 = 0.0f;
farmookong 0:5e356103dcc7 58 float pwm1_duty = 0.0f;
farmookong 0:5e356103dcc7 59 //motor 2
farmookong 0:5e356103dcc7 60 int8_t HallA_state_2 = 0;
farmookong 0:5e356103dcc7 61 int8_t HallB_state_2 = 0;
farmookong 0:5e356103dcc7 62 int8_t motor_state_2 = 0;
farmookong 0:5e356103dcc7 63 int8_t motor_state_old_2 = 0;
farmookong 0:5e356103dcc7 64 int count_2 = 0;
farmookong 0:5e356103dcc7 65 float speed_2 = 0.0f;
farmookong 0:5e356103dcc7 66 float v_ref_2 = 150.0f;
farmookong 0:5e356103dcc7 67 float v_err_2 = 0.0f;
farmookong 0:5e356103dcc7 68 float v_ierr_2 = 0.0f;
farmookong 0:5e356103dcc7 69 float ctrl_output_2 = 0.0f;
farmookong 0:5e356103dcc7 70 float pwm2_duty = 0.0f;
farmookong 0:5e356103dcc7 71 //servo
farmookong 0:5e356103dcc7 72 int i = 0;
farmookong 0:5e356103dcc7 73 //****************************************************************************** End of Variables
farmookong 0:5e356103dcc7 74
farmookong 0:5e356103dcc7 75 //****************************************************************************** Main
farmookong 0:5e356103dcc7 76 int main()
farmookong 0:5e356103dcc7 77 {
farmookong 0:5e356103dcc7 78 init_timer();
farmookong 0:5e356103dcc7 79 init_PWM();
farmookong 0:5e356103dcc7 80 init_CN();
farmookong 0:5e356103dcc7 81 while(1)
farmookong 0:5e356103dcc7 82 {
farmookong 0:5e356103dcc7 83 pc.printf("**************************\n");
farmookong 0:5e356103dcc7 84 pc.printf("speed1: %f, error: %f\n",speed_1,v_err_1);
farmookong 0:5e356103dcc7 85 pc.printf("i_error: %f ctrl output: %f \n",v_ierr_1,ctrl_output_1);
farmookong 0:5e356103dcc7 86 pc.printf("speed2: %f, error: %f\n",speed_2,v_err_2);
farmookong 0:5e356103dcc7 87 pc.printf("i_error: %f ctrl output: %f \n",v_ierr_2,ctrl_output_2);
farmookong 0:5e356103dcc7 88
farmookong 0:5e356103dcc7 89 }
farmookong 0:5e356103dcc7 90 }
farmookong 0:5e356103dcc7 91 //****************************************************************************** End of Main
farmookong 0:5e356103dcc7 92
farmookong 0:5e356103dcc7 93 //****************************************************************************** timer_interrupt
farmookong 0:5e356103dcc7 94 void timer_interrupt()
farmookong 0:5e356103dcc7 95 {
farmookong 0:5e356103dcc7 96 // Motor1
farmookong 0:5e356103dcc7 97 speed_1 = (float)count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //rpm
farmookong 0:5e356103dcc7 98 count_1 = 0;
farmookong 0:5e356103dcc7 99 // Code for PI controller //
farmookong 0:5e356103dcc7 100 v_err_1 = v_ref_1 - speed_1;
farmookong 0:5e356103dcc7 101 v_ierr_1 += (v_err_1*Ts);
farmookong 0:5e356103dcc7 102 ctrl_output_1 = 0.01f*v_err_1+ 0.1f*v_ierr_1;
farmookong 0:5e356103dcc7 103 ///////////////////////////
farmookong 0:5e356103dcc7 104
farmookong 0:5e356103dcc7 105 if(ctrl_output_1 >= 0.5f)ctrl_output_1 = 0.5f;
farmookong 0:5e356103dcc7 106 else if(ctrl_output_1 <= -0.5f)ctrl_output_1 = -0.5f;
farmookong 0:5e356103dcc7 107 pwm1_duty = ctrl_output_1 + 0.5f;
farmookong 0:5e356103dcc7 108 pwm1.write(pwm1_duty);
farmookong 0:5e356103dcc7 109 TIM1->CCER |= 0x4;
farmookong 0:5e356103dcc7 110
farmookong 0:5e356103dcc7 111 // Motor2
farmookong 0:5e356103dcc7 112 speed_2 = (float)count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //rpm
farmookong 0:5e356103dcc7 113 count_2 = 0;
farmookong 0:5e356103dcc7 114 // Code for PI controller //
farmookong 0:5e356103dcc7 115 v_err_2 = v_ref_2 - speed_2;
farmookong 0:5e356103dcc7 116 v_ierr_2 += (v_err_2*Ts);
farmookong 0:5e356103dcc7 117 ctrl_output_2 = 0.001f*v_err_2+ 0.05f*v_ierr_2;
farmookong 0:5e356103dcc7 118 ///////////////////////////
farmookong 0:5e356103dcc7 119 if(ctrl_output_2 >= 0.5f)ctrl_output_2 = 0.5f;
farmookong 0:5e356103dcc7 120 else if(ctrl_output_2 <= -0.5f)ctrl_output_2 = -0.5f;
farmookong 0:5e356103dcc7 121 pwm2_duty = ctrl_output_2 + 0.5f;
farmookong 0:5e356103dcc7 122 pwm2.write(pwm2_duty);
farmookong 0:5e356103dcc7 123 TIM1->CCER |= 0x40;
farmookong 0:5e356103dcc7 124
farmookong 0:5e356103dcc7 125 if(v_ierr_1 > 5)
farmookong 0:5e356103dcc7 126 v_ierr_1 = 0;
farmookong 0:5e356103dcc7 127 if(v_ierr_2 > 8)
farmookong 0:5e356103dcc7 128 v_ierr_2 = 0;
farmookong 0:5e356103dcc7 129
farmookong 0:5e356103dcc7 130 //Servo
farmookong 0:5e356103dcc7 131 if(i==100)
farmookong 0:5e356103dcc7 132 {
farmookong 0:5e356103dcc7 133 if(servo_duty < 0.07f)
farmookong 0:5e356103dcc7 134 {
farmookong 0:5e356103dcc7 135 servo_duty = servo_duty+0.04f/6;
farmookong 0:5e356103dcc7 136 }
farmookong 0:5e356103dcc7 137 else
farmookong 0:5e356103dcc7 138 {
farmookong 0:5e356103dcc7 139 servo_duty = 0.07f;
farmookong 0:5e356103dcc7 140 }
farmookong 0:5e356103dcc7 141 servo.write(servo_duty);
farmookong 0:5e356103dcc7 142 i=0;
farmookong 0:5e356103dcc7 143 }
farmookong 0:5e356103dcc7 144 else
farmookong 0:5e356103dcc7 145 {
farmookong 0:5e356103dcc7 146 i++;
farmookong 0:5e356103dcc7 147 }
farmookong 0:5e356103dcc7 148 }
farmookong 0:5e356103dcc7 149 //****************************************************************************** End of timer_interrupt
farmookong 0:5e356103dcc7 150
farmookong 0:5e356103dcc7 151 //****************************************************************************** CN_interrupt
farmookong 0:5e356103dcc7 152 void CN_interrupt()
farmookong 0:5e356103dcc7 153 {
farmookong 0:5e356103dcc7 154 // Motor1
farmookong 0:5e356103dcc7 155 // Read the current status of hall sensor
farmookong 0:5e356103dcc7 156 HallA_state_1 = HallA_1.read();
farmookong 0:5e356103dcc7 157 HallB_state_1 = HallB_1.read();
farmookong 0:5e356103dcc7 158
farmookong 0:5e356103dcc7 159 ///code for state determination///
farmookong 0:5e356103dcc7 160 if(HallA_state_1 == 0 && HallB_state_1 == 0)
farmookong 0:5e356103dcc7 161 motor_state_1 = 1;
farmookong 0:5e356103dcc7 162 else if(HallA_state_1 == 0 && HallB_state_1 == 1)
farmookong 0:5e356103dcc7 163 motor_state_1 = 2;
farmookong 0:5e356103dcc7 164 else if(HallA_state_1 == 1 && HallB_state_1 == 1)
farmookong 0:5e356103dcc7 165 motor_state_1 = 3;
farmookong 0:5e356103dcc7 166 else if(HallA_state_1 == 1 && HallB_state_1 == 0)
farmookong 0:5e356103dcc7 167 motor_state_1 = 4;
farmookong 0:5e356103dcc7 168
farmookong 0:5e356103dcc7 169 if(motor_state_old_1 != 0)
farmookong 0:5e356103dcc7 170 {
farmookong 0:5e356103dcc7 171 if(motor_state_old_1 < motor_state_1)
farmookong 0:5e356103dcc7 172 count_1 += 1;
farmookong 0:5e356103dcc7 173 else if(motor_state_old_1 > motor_state_1)
farmookong 0:5e356103dcc7 174 count_1 -= 1;
farmookong 0:5e356103dcc7 175 if(motor_state_old_1 == 4 && motor_state_1 == 1)
farmookong 0:5e356103dcc7 176 count_1 += 2;
farmookong 0:5e356103dcc7 177 if(motor_state_old_1 == 1 && motor_state_1 == 4)
farmookong 0:5e356103dcc7 178 count_1 -= 2;
farmookong 0:5e356103dcc7 179 }
farmookong 0:5e356103dcc7 180 motor_state_old_1 = motor_state_1;
farmookong 0:5e356103dcc7 181 //////////////////////////////////
farmookong 0:5e356103dcc7 182
farmookong 0:5e356103dcc7 183 //Forward
farmookong 0:5e356103dcc7 184 //v1Count +1
farmookong 0:5e356103dcc7 185 //Inverse
farmookong 0:5e356103dcc7 186 //v1Count -1
farmookong 0:5e356103dcc7 187
farmookong 0:5e356103dcc7 188 // Motor2
farmookong 0:5e356103dcc7 189 // Read the current status of hall sensor
farmookong 0:5e356103dcc7 190 HallA_state_2 = HallA_2.read();
farmookong 0:5e356103dcc7 191 HallB_state_2 = HallB_2.read();
farmookong 0:5e356103dcc7 192
farmookong 0:5e356103dcc7 193 ///code for state determination///
farmookong 0:5e356103dcc7 194 if(HallA_state_2 == 0 && HallB_state_2 == 0)
farmookong 0:5e356103dcc7 195 motor_state_2 = 1;
farmookong 0:5e356103dcc7 196 else if(HallA_state_2 == 0 && HallB_state_2 == 1)
farmookong 0:5e356103dcc7 197 motor_state_2 = 2;
farmookong 0:5e356103dcc7 198 else if(HallA_state_2 == 1 && HallB_state_2 == 1)
farmookong 0:5e356103dcc7 199 motor_state_2 = 3;
farmookong 0:5e356103dcc7 200 else if(HallA_state_2 == 1 && HallB_state_2 == 0)
farmookong 0:5e356103dcc7 201 motor_state_2 = 4;
farmookong 0:5e356103dcc7 202
farmookong 0:5e356103dcc7 203 if(motor_state_old_2 != 0)
farmookong 0:5e356103dcc7 204 {
farmookong 0:5e356103dcc7 205 if(motor_state_old_2 < motor_state_2)
farmookong 0:5e356103dcc7 206 count_2 += 1;
farmookong 0:5e356103dcc7 207 else if(motor_state_old_2 > motor_state_2)
farmookong 0:5e356103dcc7 208 count_2 -= 1;
farmookong 0:5e356103dcc7 209 if(motor_state_old_2 == 4 && motor_state_2 == 1)
farmookong 0:5e356103dcc7 210 count_2 += 2;
farmookong 0:5e356103dcc7 211 if(motor_state_old_2 == 1 && motor_state_2 == 4)
farmookong 0:5e356103dcc7 212 count_2 -= 2;
farmookong 0:5e356103dcc7 213 }
farmookong 0:5e356103dcc7 214 motor_state_old_2 = motor_state_2;
farmookong 0:5e356103dcc7 215
farmookong 0:5e356103dcc7 216 //////////////////////////////////
farmookong 0:5e356103dcc7 217
farmookong 0:5e356103dcc7 218 //Forward
farmookong 0:5e356103dcc7 219 //v2Count +1
farmookong 0:5e356103dcc7 220 //Inverse
farmookong 0:5e356103dcc7 221 //v2Count -1
farmookong 0:5e356103dcc7 222 }
farmookong 0:5e356103dcc7 223 //****************************************************************************** End of CN_interrupt
farmookong 0:5e356103dcc7 224
farmookong 0:5e356103dcc7 225 //****************************************************************************** init_timer
farmookong 0:5e356103dcc7 226 void init_timer()
farmookong 0:5e356103dcc7 227 {
farmookong 0:5e356103dcc7 228 timer.attach_us(&timer_interrupt, 10000);//10ms interrupt period (100 Hz)
farmookong 0:5e356103dcc7 229 }
farmookong 0:5e356103dcc7 230 //****************************************************************************** End of init_timer
farmookong 0:5e356103dcc7 231
farmookong 0:5e356103dcc7 232 //****************************************************************************** init_PWM
farmookong 0:5e356103dcc7 233 void init_PWM()
farmookong 0:5e356103dcc7 234 {
farmookong 0:5e356103dcc7 235 pwm1.period_us(50);
farmookong 0:5e356103dcc7 236 pwm1.write(0.5);
farmookong 0:5e356103dcc7 237 TIM1->CCER |= 0x4;
farmookong 0:5e356103dcc7 238
farmookong 0:5e356103dcc7 239 pwm2.period_us(50);
farmookong 0:5e356103dcc7 240 pwm2.write(0.5);
farmookong 0:5e356103dcc7 241 TIM1->CCER |= 0x40;
farmookong 0:5e356103dcc7 242
farmookong 0:5e356103dcc7 243 servo.period_ms(Servo_Period);
farmookong 0:5e356103dcc7 244 servo.write(servo_duty);
farmookong 0:5e356103dcc7 245 }
farmookong 0:5e356103dcc7 246 //****************************************************************************** End of init_PWM
farmookong 0:5e356103dcc7 247
farmookong 0:5e356103dcc7 248 //****************************************************************************** init_CN
farmookong 0:5e356103dcc7 249 void init_CN()
farmookong 0:5e356103dcc7 250 {
farmookong 0:5e356103dcc7 251 // Motor1
farmookong 0:5e356103dcc7 252 HallA_1.rise(&CN_interrupt);
farmookong 0:5e356103dcc7 253 HallA_1.fall(&CN_interrupt);
farmookong 0:5e356103dcc7 254 HallB_1.rise(&CN_interrupt);
farmookong 0:5e356103dcc7 255 HallB_1.fall(&CN_interrupt);
farmookong 0:5e356103dcc7 256
farmookong 0:5e356103dcc7 257 HallA_state_1 = HallA_1.read();
farmookong 0:5e356103dcc7 258 HallB_state_1 = HallB_1.read();
farmookong 0:5e356103dcc7 259
farmookong 0:5e356103dcc7 260 // Motor2
farmookong 0:5e356103dcc7 261 HallA_2.rise(&CN_interrupt);
farmookong 0:5e356103dcc7 262 HallA_2.fall(&CN_interrupt);
farmookong 0:5e356103dcc7 263 HallB_2.rise(&CN_interrupt);
farmookong 0:5e356103dcc7 264 HallB_2.fall(&CN_interrupt);
farmookong 0:5e356103dcc7 265
farmookong 0:5e356103dcc7 266 HallA_state_2 = HallA_2.read();
farmookong 0:5e356103dcc7 267 HallB_state_2 = HallB_2.read();
farmookong 0:5e356103dcc7 268 }
farmookong 0:5e356103dcc7 269 //****************************************************************************** End of init_CN