Xiaohai Li / Mbed 2 deprecated RPi_MOT_HAT

Raspberry Pi MOT HAT Based on STM32F030R8. The clock needs to be changed to 8MHz after export.

Dependencies:   mbed

PeripheralLayer/MotHatLib.cpp@4:fb5235d39a9c, 2020-01-06 (annotated)

Committer:
nightseas
Date:
Mon Jan 06 05:45:32 2020 +0000
Revision:
4:fb5235d39a9c
Parent:
3:171f4d0ca77b 
Merge latest official mbed lib.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nightseas 0:633cef71e6ba 1 #include "SysConfig.h"
nightseas 0:633cef71e6ba 2
nightseas 0:633cef71e6ba 3 //Number of on-board devices
nightseas 0:633cef71e6ba 4 const int LED_NUM_MAX = 1;
nightseas 0:633cef71e6ba 5 const int MOT_NUM_MAX = 4;
nightseas 0:633cef71e6ba 6 const int SRV_NUM_MAX = 4;
nightseas 0:633cef71e6ba 7 const int VMON_NUM_MAX = 4;
nightseas 0:633cef71e6ba 8
nightseas 0:633cef71e6ba 9 //The speed difference rate of left/right side motors when the vehicle turns head
nightseas 0:633cef71e6ba 10 const int MOT_TURN_RATE = 4;
nightseas 0:633cef71e6ba 11
nightseas 0:633cef71e6ba 12 //Fixed PWM period of servos
nightseas 0:633cef71e6ba 13 const int SRV_PERIOD_US = 20000;
nightseas 0:633cef71e6ba 14
nightseas 0:633cef71e6ba 15 //LEDs on mother board and daughter board
nightseas 1:a6bcf44b90df 16 //DigitalOut led_mb(LED1, 0); //Nucleo
nightseas 1:a6bcf44b90df 17 DigitalOut led_mb(PB_12, 1); //MOT HAT
nightseas 0:633cef71e6ba 18
nightseas 3:171f4d0ca77b 19 //USART2 to PC for debug
nightseas 3:171f4d0ca77b 20 Serial uart_pc(SERIAL_TX, SERIAL_RX);
nightseas 3:171f4d0ca77b 21 SerialDummy uart_dummy;
nightseas 3:171f4d0ca77b 22
nightseas 0:633cef71e6ba 23 //GPIO for L293DD motor direction control
nightseas 0:633cef71e6ba 24 //Forward = 2b'01, Backward = 2b'10, Stop = 2b'00
nightseas 0:633cef71e6ba 25 BusOut mot1_dir(PB_3, PA_11), mot2_dir(PA_15, PA_12), mot3_dir(PD_2, PC_12), mot4_dir(PB_6, PB_7);
nightseas 0:633cef71e6ba 26
nightseas 1:a6bcf44b90df 27 //GPIO for motor velocimeter
nightseas 1:a6bcf44b90df 28 InterruptIn spd1_in(PF_6), spd2_in(PF_7), spd3_in(PF_5), spd4_in(PC_4);
nightseas 1:a6bcf44b90df 29 //GPIO for ultrasound rangefinder
nightseas 1:a6bcf44b90df 30 InterruptIn us1_echo(PC_10), us2_echo(PC_5);
nightseas 1:a6bcf44b90df 31 DigitalOut us1_trig(PC_11), us2_trig(PF_4);
nightseas 1:a6bcf44b90df 32
nightseas 1:a6bcf44b90df 33 //Timers for speed and distance counter
nightseas 1:a6bcf44b90df 34 Timer timer_spd, timer_us;
nightseas 1:a6bcf44b90df 35
nightseas 0:633cef71e6ba 36 //Analog inputs for power voltage monitoring
nightseas 0:633cef71e6ba 37 AnalogIn vin_bat(PC_0); //ADC1_CH10
nightseas 0:633cef71e6ba 38 AnalogIn vin_12v(PC_1); //ADC1_CH11
nightseas 0:633cef71e6ba 39 AnalogIn vin_5v(PC_2); //ADC1_CH12
nightseas 0:633cef71e6ba 40 AnalogIn vin_3v3(PC_3); //ADC1_CH13
nightseas 0:633cef71e6ba 41
nightseas 0:633cef71e6ba 42 //TIM3 PWM channels
nightseas 0:633cef71e6ba 43 PwmOut mot1_pwm(PB_5); //TIM3_CH2
nightseas 0:633cef71e6ba 44 PwmOut mot2_pwm(PB_4); //TIM3_CH1
nightseas 0:633cef71e6ba 45 PwmOut mot3_pwm(PB_0); //TIM3_CH3
nightseas 0:633cef71e6ba 46 PwmOut mot4_pwm(PC_9); //TIM3_CH4
nightseas 0:633cef71e6ba 47
nightseas 0:633cef71e6ba 48 //TIM15 PWM channels
nightseas 0:633cef71e6ba 49 PwmOut servo1_pwm(PB_14); //TIM15_CH1
nightseas 0:633cef71e6ba 50 PwmOut servo2_pwm(PB_15); //TIM15_CH2
nightseas 0:633cef71e6ba 51 //TIM16 PWM channels
nightseas 3:171f4d0ca77b 52 PwmOut servo3_pwm(PB_8); //TIM16_CH1
nightseas 0:633cef71e6ba 53 //TIM17 PWM channels
nightseas 3:171f4d0ca77b 54 PwmOut servo4_pwm(PB_9); //TIM17_CH1
nightseas 0:633cef71e6ba 55
nightseas 0:633cef71e6ba 56 //Position calibration for servos
nightseas 0:633cef71e6ba 57 //ServoCal[x][0] is -90 degrees, ServoCal[x][1] is +90 degrees
nightseas 0:633cef71e6ba 58 const int ServoCal[][2] = {{1000, 2000}, {1000, 2000}, {1000, 2000}, {1000, 2000}};
nightseas 0:633cef71e6ba 59
nightseas 0:633cef71e6ba 60 //Voltage division ratios
nightseas 0:633cef71e6ba 61 const float VmonDivision[] = {0.1, 0.1, 0.5, 0.5};
nightseas 0:633cef71e6ba 62
nightseas 0:633cef71e6ba 63 //Init board library
nightseas 0:633cef71e6ba 64 //--> Success return 0
nightseas 0:633cef71e6ba 65 int BoardLibInit(void)
nightseas 0:633cef71e6ba 66 {
nightseas 0:633cef71e6ba 67 //Init LED status
nightseas 0:633cef71e6ba 68 LedOnAll();
nightseas 0:633cef71e6ba 69
nightseas 0:633cef71e6ba 70 //Init debug UART
nightseas 3:171f4d0ca77b 71 uart_pc.baud(115200);
nightseas 0:633cef71e6ba 72
nightseas 0:633cef71e6ba 73 //Init DC motors
nightseas 0:633cef71e6ba 74 Mot_Init();
nightseas 0:633cef71e6ba 75
nightseas 0:633cef71e6ba 76 //Init servos
nightseas 0:633cef71e6ba 77 Servo_Init();
nightseas 0:633cef71e6ba 78
nightseas 0:633cef71e6ba 79 //Return 0 if success
nightseas 0:633cef71e6ba 80 return 0;
nightseas 0:633cef71e6ba 81 }
nightseas 0:633cef71e6ba 82
nightseas 0:633cef71e6ba 83 /*********************************************/
nightseas 0:633cef71e6ba 84 /* Voltage Monitor Functions */
nightseas 0:633cef71e6ba 85 /*********************************************/
nightseas 0:633cef71e6ba 86 float Vmon_ReadVolt(int channel)
nightseas 0:633cef71e6ba 87 {
nightseas 0:633cef71e6ba 88 switch(channel)
nightseas 0:633cef71e6ba 89 {
nightseas 0:633cef71e6ba 90 case 1:
nightseas 0:633cef71e6ba 91 return vin_bat.read() * 3.3 / VmonDivision[channel - 1];
nightseas 0:633cef71e6ba 92
nightseas 0:633cef71e6ba 93 case 2:
nightseas 0:633cef71e6ba 94 return vin_12v.read() * 3.3 / VmonDivision[channel - 1];
nightseas 0:633cef71e6ba 95
nightseas 0:633cef71e6ba 96 case 3:
nightseas 0:633cef71e6ba 97 return vin_5v.read() * 3.3 / VmonDivision[channel - 1];
nightseas 0:633cef71e6ba 98
nightseas 0:633cef71e6ba 99 case 4:
nightseas 0:633cef71e6ba 100 return vin_3v3.read() * 3.3 / VmonDivision[channel - 1];
nightseas 0:633cef71e6ba 101
nightseas 0:633cef71e6ba 102 default:
nightseas 0:633cef71e6ba 103 return 0;
nightseas 0:633cef71e6ba 104 }
nightseas 0:633cef71e6ba 105 }
nightseas 0:633cef71e6ba 106
nightseas 0:633cef71e6ba 107
nightseas 0:633cef71e6ba 108 /*********************************************/
nightseas 0:633cef71e6ba 109 /* DC Motor Control Functions */
nightseas 0:633cef71e6ba 110 /*********************************************/
nightseas 0:633cef71e6ba 111 //Init the PWM and direction of motors to STOP state
nightseas 0:633cef71e6ba 112 void Mot_Init(void)
nightseas 0:633cef71e6ba 113 {
nightseas 0:633cef71e6ba 114 for(int motNum = 1; motNum <= MOT_NUM_MAX; motNum++)
nightseas 0:633cef71e6ba 115 {
nightseas 0:633cef71e6ba 116 Mot_PwmSetup(motNum, 1000, 0); //Freq = 1KHz, Speed = 0%
nightseas 0:633cef71e6ba 117 Mot_SetDirection(motNum, 's'); //Direction = STOP
nightseas 0:633cef71e6ba 118 }
nightseas 0:633cef71e6ba 119 }
nightseas 0:633cef71e6ba 120
nightseas 0:633cef71e6ba 121 //Control the movement of vehicle
nightseas 0:633cef71e6ba 122 //dir: forward = 'f', backward = 'b', stop = 's'
nightseas 0:633cef71e6ba 123 //turn: left = 'l', right = 'r', fixed turning rate
nightseas 0:633cef71e6ba 124 //speed: 0.0~1.0 (0~100%, 0.245 = 24.5%)
nightseas 0:633cef71e6ba 125 void Mot_Ctrl(char dir, char turn, float speed)
nightseas 0:633cef71e6ba 126 {
nightseas 0:633cef71e6ba 127 if(speed < 0)
nightseas 0:633cef71e6ba 128 speed = 0;
nightseas 0:633cef71e6ba 129 if(speed > 1)
nightseas 0:633cef71e6ba 130 speed = 1;
nightseas 0:633cef71e6ba 131
nightseas 0:633cef71e6ba 132 switch(turn)
nightseas 0:633cef71e6ba 133 {
nightseas 1:a6bcf44b90df 134 case 'm':
nightseas 1:a6bcf44b90df 135 Mot_PwmWrite(1, speed);
nightseas 1:a6bcf44b90df 136 Mot_SetDirection(1, dir);
nightseas 1:a6bcf44b90df 137 Mot_PwmWrite(2, speed);
nightseas 1:a6bcf44b90df 138 Mot_SetDirection(2, dir);
nightseas 1:a6bcf44b90df 139
nightseas 1:a6bcf44b90df 140 if(MOT_NUM_MAX > 2)
nightseas 1:a6bcf44b90df 141 {
nightseas 1:a6bcf44b90df 142 Mot_PwmWrite(3, speed);
nightseas 1:a6bcf44b90df 143 Mot_SetDirection(3, dir);
nightseas 1:a6bcf44b90df 144 Mot_PwmWrite(4, speed);
nightseas 1:a6bcf44b90df 145 Mot_SetDirection(4, dir);
nightseas 1:a6bcf44b90df 146 }
nightseas 1:a6bcf44b90df 147 break;
nightseas 1:a6bcf44b90df 148
nightseas 0:633cef71e6ba 149 case 'l':
nightseas 0:633cef71e6ba 150 Mot_PwmWrite(1, speed / MOT_TURN_RATE);
nightseas 0:633cef71e6ba 151 Mot_SetDirection(1, dir);
nightseas 0:633cef71e6ba 152 Mot_PwmWrite(2, speed);
nightseas 0:633cef71e6ba 153 Mot_SetDirection(2, dir);
nightseas 0:633cef71e6ba 154
nightseas 0:633cef71e6ba 155 if(MOT_NUM_MAX > 2)
nightseas 0:633cef71e6ba 156 {
nightseas 0:633cef71e6ba 157 Mot_PwmWrite(3, speed / MOT_TURN_RATE);
nightseas 0:633cef71e6ba 158 Mot_SetDirection(3, dir);
nightseas 0:633cef71e6ba 159 Mot_PwmWrite(4, speed);
nightseas 0:633cef71e6ba 160 Mot_SetDirection(4, dir);
nightseas 0:633cef71e6ba 161 }
nightseas 0:633cef71e6ba 162 break;
nightseas 0:633cef71e6ba 163
nightseas 0:633cef71e6ba 164 case 'r':
nightseas 0:633cef71e6ba 165 Mot_PwmWrite(1, speed);
nightseas 0:633cef71e6ba 166 Mot_SetDirection(1, dir);
nightseas 0:633cef71e6ba 167 Mot_PwmWrite(2, speed / MOT_TURN_RATE);
nightseas 0:633cef71e6ba 168 Mot_SetDirection(2, dir);
nightseas 0:633cef71e6ba 169
nightseas 0:633cef71e6ba 170 if(MOT_NUM_MAX > 2)
nightseas 0:633cef71e6ba 171 {
nightseas 0:633cef71e6ba 172 Mot_PwmWrite(3, speed);
nightseas 0:633cef71e6ba 173 Mot_SetDirection(3, dir);
nightseas 0:633cef71e6ba 174 Mot_PwmWrite(4, speed / MOT_TURN_RATE);
nightseas 0:633cef71e6ba 175 Mot_SetDirection(4, dir);
nightseas 0:633cef71e6ba 176 }
nightseas 0:633cef71e6ba 177 break;
nightseas 0:633cef71e6ba 178
nightseas 0:633cef71e6ba 179 case 's':
nightseas 0:633cef71e6ba 180 for(int motNum = 1; motNum <= MOT_NUM_MAX; motNum++)
nightseas 0:633cef71e6ba 181 {
nightseas 0:633cef71e6ba 182 Mot_PwmWrite(motNum, 0);
nightseas 0:633cef71e6ba 183 Mot_SetDirection(motNum, 's');
nightseas 0:633cef71e6ba 184 }
nightseas 0:633cef71e6ba 185 break;
nightseas 0:633cef71e6ba 186 }
nightseas 0:633cef71e6ba 187 }
nightseas 0:633cef71e6ba 188
nightseas 0:633cef71e6ba 189 //channel: 1~4, period: us, duty: 0.0~1.0 (0.245 = 24.5%)
nightseas 0:633cef71e6ba 190 void Mot_PwmSetup(int channel, int period, float duty)
nightseas 0:633cef71e6ba 191 {
nightseas 0:633cef71e6ba 192 if(duty < 0)
nightseas 0:633cef71e6ba 193 duty = 0;
nightseas 0:633cef71e6ba 194 else if(duty > 1)
nightseas 0:633cef71e6ba 195 duty = 1;
nightseas 0:633cef71e6ba 196
nightseas 0:633cef71e6ba 197 switch(channel)
nightseas 0:633cef71e6ba 198 {
nightseas 0:633cef71e6ba 199 case 1:
nightseas 0:633cef71e6ba 200 mot1_pwm.period_us(period);
nightseas 0:633cef71e6ba 201 mot1_pwm = duty;
nightseas 0:633cef71e6ba 202 break;
nightseas 0:633cef71e6ba 203 case 2:
nightseas 0:633cef71e6ba 204 mot2_pwm.period_us(period);
nightseas 0:633cef71e6ba 205 mot2_pwm = duty;
nightseas 0:633cef71e6ba 206 break;
nightseas 0:633cef71e6ba 207 case 3:
nightseas 0:633cef71e6ba 208 mot3_pwm.period_us(period);
nightseas 0:633cef71e6ba 209 mot3_pwm = duty;
nightseas 0:633cef71e6ba 210 break;
nightseas 0:633cef71e6ba 211 case 4:
nightseas 0:633cef71e6ba 212 mot4_pwm.period_us(period);
nightseas 0:633cef71e6ba 213 mot4_pwm = duty;
nightseas 0:633cef71e6ba 214 break;
nightseas 0:633cef71e6ba 215
nightseas 0:633cef71e6ba 216 default:
nightseas 0:633cef71e6ba 217 break;
nightseas 0:633cef71e6ba 218 }
nightseas 0:633cef71e6ba 219 }
nightseas 0:633cef71e6ba 220
nightseas 0:633cef71e6ba 221 //channel: 1~4, duty: 0.0~1.0 (0.245 = 24.5%)
nightseas 0:633cef71e6ba 222 void Mot_PwmWrite(int channel, float duty)
nightseas 0:633cef71e6ba 223 {
nightseas 0:633cef71e6ba 224 if(duty < 0)
nightseas 0:633cef71e6ba 225 duty = 0;
nightseas 0:633cef71e6ba 226 else if(duty > 1)
nightseas 0:633cef71e6ba 227 duty = 1;
nightseas 0:633cef71e6ba 228
nightseas 0:633cef71e6ba 229 switch(channel)
nightseas 0:633cef71e6ba 230 {
nightseas 0:633cef71e6ba 231 //All channels in TIM3 share the same period
nightseas 0:633cef71e6ba 232 case 1:
nightseas 0:633cef71e6ba 233 mot1_pwm = duty;
nightseas 0:633cef71e6ba 234 break;
nightseas 0:633cef71e6ba 235 case 2:
nightseas 0:633cef71e6ba 236 mot2_pwm = duty;
nightseas 0:633cef71e6ba 237 break;
nightseas 0:633cef71e6ba 238 case 3:
nightseas 0:633cef71e6ba 239 mot3_pwm = duty;
nightseas 0:633cef71e6ba 240 break;
nightseas 0:633cef71e6ba 241 case 4:
nightseas 0:633cef71e6ba 242 mot4_pwm = duty;
nightseas 0:633cef71e6ba 243 break;
nightseas 0:633cef71e6ba 244
nightseas 0:633cef71e6ba 245 default:
nightseas 0:633cef71e6ba 246 break;
nightseas 0:633cef71e6ba 247 }
nightseas 0:633cef71e6ba 248 }
nightseas 0:633cef71e6ba 249
nightseas 0:633cef71e6ba 250 //channel: 1~4, dir: forward = 'f', backward = 'b', stop = 's'
nightseas 0:633cef71e6ba 251 void Mot_SetDirection(int channel, char dir)
nightseas 0:633cef71e6ba 252 {
nightseas 0:633cef71e6ba 253 switch(channel)
nightseas 0:633cef71e6ba 254 {
nightseas 0:633cef71e6ba 255 case 1:
nightseas 0:633cef71e6ba 256 if(dir == 'f')
nightseas 0:633cef71e6ba 257 mot1_dir = 0x1;
nightseas 0:633cef71e6ba 258 else if(dir == 'b')
nightseas 0:633cef71e6ba 259 mot1_dir = 0x2;
nightseas 0:633cef71e6ba 260 else
nightseas 0:633cef71e6ba 261 mot1_dir = 0x0;
nightseas 0:633cef71e6ba 262 break;
nightseas 0:633cef71e6ba 263 case 2:
nightseas 0:633cef71e6ba 264 if(dir == 'f')
nightseas 0:633cef71e6ba 265 mot2_dir = 0x1;
nightseas 0:633cef71e6ba 266 else if(dir == 'b')
nightseas 0:633cef71e6ba 267 mot2_dir = 0x2;
nightseas 0:633cef71e6ba 268 else
nightseas 0:633cef71e6ba 269 mot2_dir = 0x0;
nightseas 0:633cef71e6ba 270 break;
nightseas 0:633cef71e6ba 271 case 3:
nightseas 0:633cef71e6ba 272 if(dir == 'f')
nightseas 0:633cef71e6ba 273 mot3_dir = 0x1;
nightseas 0:633cef71e6ba 274 else if(dir == 'b')
nightseas 0:633cef71e6ba 275 mot3_dir = 0x2;
nightseas 0:633cef71e6ba 276 else
nightseas 0:633cef71e6ba 277 mot3_dir = 0x0;
nightseas 0:633cef71e6ba 278 break;
nightseas 0:633cef71e6ba 279 case 4:
nightseas 0:633cef71e6ba 280 if(dir == 'f')
nightseas 0:633cef71e6ba 281 mot4_dir = 0x1;
nightseas 0:633cef71e6ba 282 else if(dir == 'b')
nightseas 0:633cef71e6ba 283 mot4_dir = 0x2;
nightseas 0:633cef71e6ba 284 else
nightseas 0:633cef71e6ba 285 mot4_dir = 0x0;
nightseas 0:633cef71e6ba 286 break;
nightseas 0:633cef71e6ba 287 }
nightseas 0:633cef71e6ba 288 }
nightseas 0:633cef71e6ba 289
nightseas 0:633cef71e6ba 290 /*********************************************/
nightseas 1:a6bcf44b90df 291 /* Motor Velocimetoer Functions */
nightseas 1:a6bcf44b90df 292 /*********************************************/
nightseas 1:a6bcf44b90df 293 int MotSpdPulseTime[] = {0, 0, 0, 0};
nightseas 1:a6bcf44b90df 294 int MotSpeed[] = {0, 0, 0, 0};
nightseas 1:a6bcf44b90df 295
nightseas 1:a6bcf44b90df 296 void Mot_StartVelocimeter(void)
nightseas 1:a6bcf44b90df 297 {
nightseas 1:a6bcf44b90df 298 timer_spd.start();
nightseas 1:a6bcf44b90df 299
nightseas 1:a6bcf44b90df 300 spd1_in.rise(&Mot_SpdPluseIntHandler1);
nightseas 1:a6bcf44b90df 301 spd2_in.rise(&Mot_SpdPluseIntHandler2);
nightseas 1:a6bcf44b90df 302 spd3_in.rise(&Mot_SpdPluseIntHandler3);
nightseas 1:a6bcf44b90df 303 spd4_in.rise(&Mot_SpdPluseIntHandler4);
nightseas 1:a6bcf44b90df 304 }
nightseas 1:a6bcf44b90df 305
nightseas 1:a6bcf44b90df 306 void Mot_StopVelocimeter(void)
nightseas 1:a6bcf44b90df 307 {
nightseas 1:a6bcf44b90df 308 spd1_in.disable_irq();
nightseas 1:a6bcf44b90df 309 spd2_in.disable_irq();
nightseas 1:a6bcf44b90df 310 spd3_in.disable_irq();
nightseas 1:a6bcf44b90df 311 spd4_in.disable_irq();
nightseas 1:a6bcf44b90df 312
nightseas 1:a6bcf44b90df 313 timer_spd.stop();
nightseas 3:171f4d0ca77b 314 for(int i=1; i<=MOT_NUM_MAX; i++)
nightseas 3:171f4d0ca77b 315 MotSpdPulseTime[i-1] = 0;
nightseas 1:a6bcf44b90df 316 }
nightseas 1:a6bcf44b90df 317
nightseas 1:a6bcf44b90df 318 int Mot_GetSpeed(int channel)
nightseas 1:a6bcf44b90df 319 {
nightseas 1:a6bcf44b90df 320 if(channel < 1)
nightseas 1:a6bcf44b90df 321 channel = 1;
nightseas 1:a6bcf44b90df 322 else if(channel > MOT_NUM_MAX)
nightseas 1:a6bcf44b90df 323 channel = MOT_NUM_MAX;
nightseas 1:a6bcf44b90df 324
nightseas 1:a6bcf44b90df 325 return MotSpeed[channel - 1];
nightseas 1:a6bcf44b90df 326 }
nightseas 1:a6bcf44b90df 327
nightseas 1:a6bcf44b90df 328 void Mot_SpdPluseIntHandler1(void)
nightseas 1:a6bcf44b90df 329 {
nightseas 1:a6bcf44b90df 330 int time = timer_spd.read_ms();
nightseas 1:a6bcf44b90df 331 MotSpeed[0] = 3000 / (time - MotSpdPulseTime[0]);
nightseas 1:a6bcf44b90df 332 MotSpdPulseTime[0] = time;
nightseas 1:a6bcf44b90df 333 }
nightseas 1:a6bcf44b90df 334
nightseas 1:a6bcf44b90df 335 void Mot_SpdPluseIntHandler2(void)
nightseas 1:a6bcf44b90df 336 {
nightseas 1:a6bcf44b90df 337 int time = timer_spd.read_ms();
nightseas 1:a6bcf44b90df 338 MotSpeed[1] = 3000 / (time - MotSpdPulseTime[1]);
nightseas 1:a6bcf44b90df 339 MotSpdPulseTime[1] = time;
nightseas 1:a6bcf44b90df 340 }
nightseas 1:a6bcf44b90df 341
nightseas 1:a6bcf44b90df 342 void Mot_SpdPluseIntHandler3(void)
nightseas 1:a6bcf44b90df 343 {
nightseas 1:a6bcf44b90df 344 int time = timer_spd.read_ms();
nightseas 1:a6bcf44b90df 345 MotSpeed[2] = 3000 / (time - MotSpdPulseTime[2]);
nightseas 1:a6bcf44b90df 346 MotSpdPulseTime[2] = time;
nightseas 1:a6bcf44b90df 347 }
nightseas 1:a6bcf44b90df 348
nightseas 1:a6bcf44b90df 349 void Mot_SpdPluseIntHandler4(void)
nightseas 1:a6bcf44b90df 350 {
nightseas 1:a6bcf44b90df 351 int time = timer_spd.read_ms();
nightseas 1:a6bcf44b90df 352 MotSpeed[3] = 3000 / (time - MotSpdPulseTime[3]);
nightseas 1:a6bcf44b90df 353 MotSpdPulseTime[3] = time;
nightseas 1:a6bcf44b90df 354 }
nightseas 1:a6bcf44b90df 355
nightseas 1:a6bcf44b90df 356 /*********************************************/
nightseas 0:633cef71e6ba 357 /* Servo Control Functions */
nightseas 0:633cef71e6ba 358 /*********************************************/
nightseas 0:633cef71e6ba 359 //Init the PWM and direction of motors to STOP state
nightseas 0:633cef71e6ba 360 void Servo_Init(void)
nightseas 0:633cef71e6ba 361 {
nightseas 0:633cef71e6ba 362 for(int servoNum = 1; servoNum <= SRV_NUM_MAX; servoNum++)
nightseas 0:633cef71e6ba 363 {
nightseas 0:633cef71e6ba 364 Servo_SetAngle(servoNum, 0);
nightseas 0:633cef71e6ba 365 }
nightseas 0:633cef71e6ba 366 }
nightseas 0:633cef71e6ba 367
nightseas 0:633cef71e6ba 368 //channel: 1~4, angle: -90.0~+90.0
nightseas 0:633cef71e6ba 369 void Servo_SetAngle(int channel, float angle)
nightseas 0:633cef71e6ba 370 {
nightseas 0:633cef71e6ba 371 if(channel > 0 && channel <= SRV_NUM_MAX)
nightseas 0:633cef71e6ba 372 {
nightseas 0:633cef71e6ba 373 int pulse = (int)((angle + 90.0) * (ServoCal[channel - 1][1] - ServoCal[channel-1][0]) / 180.0 + ServoCal[channel-1][0]);
nightseas 0:633cef71e6ba 374 Servo_PwmWrite(channel, pulse);
nightseas 0:633cef71e6ba 375 }
nightseas 0:633cef71e6ba 376 }
nightseas 0:633cef71e6ba 377
nightseas 0:633cef71e6ba 378 //channel: 1~4, pulse_us: 0~20000us
nightseas 0:633cef71e6ba 379 void Servo_PwmSetup(int channel, int pulse_us)
nightseas 0:633cef71e6ba 380 {
nightseas 0:633cef71e6ba 381 if(pulse_us < 0)
nightseas 0:633cef71e6ba 382 pulse_us = 0;
nightseas 0:633cef71e6ba 383 else if(pulse_us > 20000)
nightseas 0:633cef71e6ba 384 pulse_us = 20000;
nightseas 0:633cef71e6ba 385
nightseas 0:633cef71e6ba 386 switch(channel)
nightseas 0:633cef71e6ba 387 {
nightseas 0:633cef71e6ba 388 case 1:
nightseas 0:633cef71e6ba 389 servo1_pwm.period_us(SRV_PERIOD_US);
nightseas 0:633cef71e6ba 390 servo1_pwm.pulsewidth_us(pulse_us);
nightseas 0:633cef71e6ba 391 break;
nightseas 0:633cef71e6ba 392 case 2:
nightseas 0:633cef71e6ba 393 servo2_pwm.period_us(SRV_PERIOD_US);
nightseas 0:633cef71e6ba 394 servo2_pwm.pulsewidth_us(pulse_us);
nightseas 0:633cef71e6ba 395 break;
nightseas 0:633cef71e6ba 396 case 3:
nightseas 0:633cef71e6ba 397 servo3_pwm.period_us(SRV_PERIOD_US);
nightseas 0:633cef71e6ba 398 servo3_pwm.pulsewidth_us(pulse_us);
nightseas 0:633cef71e6ba 399 break;
nightseas 0:633cef71e6ba 400 case 4:
nightseas 0:633cef71e6ba 401 servo4_pwm.period_us(SRV_PERIOD_US);
nightseas 0:633cef71e6ba 402 servo4_pwm.pulsewidth_us(pulse_us);
nightseas 0:633cef71e6ba 403 break;
nightseas 0:633cef71e6ba 404
nightseas 0:633cef71e6ba 405 default:
nightseas 0:633cef71e6ba 406 break;
nightseas 0:633cef71e6ba 407 }
nightseas 0:633cef71e6ba 408 }
nightseas 0:633cef71e6ba 409
nightseas 0:633cef71e6ba 410 //channel: 1~4, pulse_us: 0~20000us
nightseas 0:633cef71e6ba 411 void Servo_PwmWrite(int channel, int pulse_us)
nightseas 0:633cef71e6ba 412 {
nightseas 0:633cef71e6ba 413 if(pulse_us < 0)
nightseas 0:633cef71e6ba 414 pulse_us = 0;
nightseas 0:633cef71e6ba 415 else if(pulse_us > 20000)
nightseas 0:633cef71e6ba 416 pulse_us = 20000;
nightseas 0:633cef71e6ba 417
nightseas 0:633cef71e6ba 418 switch(channel)
nightseas 0:633cef71e6ba 419 {
nightseas 0:633cef71e6ba 420 //All channels in TIM3 share the same period
nightseas 0:633cef71e6ba 421 case 1:
nightseas 0:633cef71e6ba 422 servo1_pwm.pulsewidth_us(pulse_us);
nightseas 0:633cef71e6ba 423 break;
nightseas 0:633cef71e6ba 424 case 2:
nightseas 0:633cef71e6ba 425 servo2_pwm.pulsewidth_us(pulse_us);
nightseas 0:633cef71e6ba 426 break;
nightseas 0:633cef71e6ba 427 case 3:
nightseas 0:633cef71e6ba 428 servo3_pwm.pulsewidth_us(pulse_us);
nightseas 0:633cef71e6ba 429 break;
nightseas 0:633cef71e6ba 430 case 4:
nightseas 0:633cef71e6ba 431 servo4_pwm.pulsewidth_us(pulse_us);
nightseas 0:633cef71e6ba 432 break;
nightseas 0:633cef71e6ba 433
nightseas 0:633cef71e6ba 434 default:
nightseas 0:633cef71e6ba 435 break;
nightseas 0:633cef71e6ba 436 }
nightseas 0:633cef71e6ba 437 }
nightseas 0:633cef71e6ba 438
nightseas 0:633cef71e6ba 439 /*********************************************/
nightseas 1:a6bcf44b90df 440 /* Ultrasound Rangefinder Functions */
nightseas 1:a6bcf44b90df 441 /*********************************************/
nightseas 1:a6bcf44b90df 442 //xxx
nightseas 1:a6bcf44b90df 443
nightseas 1:a6bcf44b90df 444
nightseas 1:a6bcf44b90df 445 /*********************************************/
nightseas 0:633cef71e6ba 446 /* LED Control Functions */
nightseas 0:633cef71e6ba 447 /*********************************************/
nightseas 0:633cef71e6ba 448 void LedOn(int ch)
nightseas 0:633cef71e6ba 449 {
nightseas 0:633cef71e6ba 450 switch(ch)
nightseas 0:633cef71e6ba 451 {
nightseas 0:633cef71e6ba 452 case 0:
nightseas 3:171f4d0ca77b 453 led_mb = 0;
nightseas 0:633cef71e6ba 454 break;
nightseas 0:633cef71e6ba 455
nightseas 0:633cef71e6ba 456 default:
nightseas 0:633cef71e6ba 457 break;
nightseas 0:633cef71e6ba 458 }
nightseas 0:633cef71e6ba 459 }
nightseas 0:633cef71e6ba 460
nightseas 0:633cef71e6ba 461 void LedOff(int ch)
nightseas 0:633cef71e6ba 462 {
nightseas 0:633cef71e6ba 463 switch(ch)
nightseas 0:633cef71e6ba 464 {
nightseas 0:633cef71e6ba 465 case 0:
nightseas 3:171f4d0ca77b 466 led_mb = 1;
nightseas 0:633cef71e6ba 467 break;
nightseas 0:633cef71e6ba 468
nightseas 0:633cef71e6ba 469 default:
nightseas 0:633cef71e6ba 470 break;
nightseas 0:633cef71e6ba 471 }
nightseas 0:633cef71e6ba 472 }
nightseas 0:633cef71e6ba 473
nightseas 0:633cef71e6ba 474 void LedToggle(int ch)
nightseas 0:633cef71e6ba 475 {
nightseas 0:633cef71e6ba 476 switch(ch)
nightseas 0:633cef71e6ba 477 {
nightseas 0:633cef71e6ba 478 case 0:
nightseas 0:633cef71e6ba 479 led_mb = !led_mb;
nightseas 0:633cef71e6ba 480 break;
nightseas 0:633cef71e6ba 481
nightseas 0:633cef71e6ba 482 default:
nightseas 0:633cef71e6ba 483 break;
nightseas 0:633cef71e6ba 484 }
nightseas 0:633cef71e6ba 485 }
nightseas 0:633cef71e6ba 486
nightseas 0:633cef71e6ba 487 void LedOnAll(void)
nightseas 0:633cef71e6ba 488 {
nightseas 0:633cef71e6ba 489 for(int ledNum = 0; ledNum < LED_NUM_MAX; ledNum++)
nightseas 0:633cef71e6ba 490 LedOn(ledNum);
nightseas 0:633cef71e6ba 491 }
nightseas 0:633cef71e6ba 492
nightseas 0:633cef71e6ba 493 void LedOffAll(void)
nightseas 0:633cef71e6ba 494 {
nightseas 0:633cef71e6ba 495 for(int ledNum = 0; ledNum < LED_NUM_MAX; ledNum++)
nightseas 0:633cef71e6ba 496 LedOff(ledNum);
nightseas 0:633cef71e6ba 497 }