Xiaohai Li / Mbed 2 deprecated RPi_MOT_HAT

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

PeripheralLayer/MotHatLib.cpp@0:633cef71e6ba, 2015-08-09 (annotated)

Committer:
nightseas
Date:
Sun Aug 09 09:37:55 2015 +0000
Revision:
0:633cef71e6ba
Child:
1:a6bcf44b90df
Basic DC motor and servo control lib finished!

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 0:633cef71e6ba 16 DigitalOut led_mb(LED1, 0); //Nucleo
nightseas 0:633cef71e6ba 17 //DigitalOut led_mb(PB_12, 1); //MOT HAT
nightseas 0:633cef71e6ba 18
nightseas 0:633cef71e6ba 19 //GPIO for L293DD motor direction control
nightseas 0:633cef71e6ba 20 //Forward = 2b'01, Backward = 2b'10, Stop = 2b'00
nightseas 0:633cef71e6ba 21 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 22
nightseas 0:633cef71e6ba 23 //Analog inputs for power voltage monitoring
nightseas 0:633cef71e6ba 24 AnalogIn vin_bat(PC_0); //ADC1_CH10
nightseas 0:633cef71e6ba 25 AnalogIn vin_12v(PC_1); //ADC1_CH11
nightseas 0:633cef71e6ba 26 AnalogIn vin_5v(PC_2); //ADC1_CH12
nightseas 0:633cef71e6ba 27 AnalogIn vin_3v3(PC_3); //ADC1_CH13
nightseas 0:633cef71e6ba 28
nightseas 0:633cef71e6ba 29 //TIM3 PWM channels
nightseas 0:633cef71e6ba 30 PwmOut mot1_pwm(PB_5); //TIM3_CH2
nightseas 0:633cef71e6ba 31 PwmOut mot2_pwm(PB_4); //TIM3_CH1
nightseas 0:633cef71e6ba 32 PwmOut mot3_pwm(PB_0); //TIM3_CH3
nightseas 0:633cef71e6ba 33 PwmOut mot4_pwm(PC_9); //TIM3_CH4
nightseas 0:633cef71e6ba 34
nightseas 0:633cef71e6ba 35 //TIM15 PWM channels
nightseas 0:633cef71e6ba 36 PwmOut servo1_pwm(PB_14); //TIM15_CH1
nightseas 0:633cef71e6ba 37 PwmOut servo2_pwm(PB_15); //TIM15_CH2
nightseas 0:633cef71e6ba 38 //TIM16 PWM channels
nightseas 0:633cef71e6ba 39 PwmOut servo3_pwm(PA_6); //TIM17_CH1
nightseas 0:633cef71e6ba 40 //TIM17 PWM channels
nightseas 0:633cef71e6ba 41 PwmOut servo4_pwm(PA_7); //TIM17_CH1
nightseas 0:633cef71e6ba 42
nightseas 0:633cef71e6ba 43 //Position calibration for servos
nightseas 0:633cef71e6ba 44 //ServoCal[x][0] is -90 degrees, ServoCal[x][1] is +90 degrees
nightseas 0:633cef71e6ba 45 const int ServoCal[][2] = {{1000, 2000}, {1000, 2000}, {1000, 2000}, {1000, 2000}};
nightseas 0:633cef71e6ba 46
nightseas 0:633cef71e6ba 47 //Voltage division ratios
nightseas 0:633cef71e6ba 48 const float VmonDivision[] = {0.1, 0.1, 0.5, 0.5};
nightseas 0:633cef71e6ba 49
nightseas 0:633cef71e6ba 50 //Init board library
nightseas 0:633cef71e6ba 51 //--> Success return 0
nightseas 0:633cef71e6ba 52 int BoardLibInit(void)
nightseas 0:633cef71e6ba 53 {
nightseas 0:633cef71e6ba 54 //Init LED status
nightseas 0:633cef71e6ba 55 LedOnAll();
nightseas 0:633cef71e6ba 56
nightseas 0:633cef71e6ba 57 //Init debug UART
nightseas 0:633cef71e6ba 58 //baud(115200);
nightseas 0:633cef71e6ba 59
nightseas 0:633cef71e6ba 60 //Init DC motors
nightseas 0:633cef71e6ba 61 Mot_Init();
nightseas 0:633cef71e6ba 62
nightseas 0:633cef71e6ba 63 //Init servos
nightseas 0:633cef71e6ba 64 Servo_Init();
nightseas 0:633cef71e6ba 65
nightseas 0:633cef71e6ba 66 //Return 0 if success
nightseas 0:633cef71e6ba 67 return 0;
nightseas 0:633cef71e6ba 68 }
nightseas 0:633cef71e6ba 69
nightseas 0:633cef71e6ba 70 /*********************************************/
nightseas 0:633cef71e6ba 71 /* Voltage Monitor Functions */
nightseas 0:633cef71e6ba 72 /*********************************************/
nightseas 0:633cef71e6ba 73 float Vmon_ReadVolt(int channel)
nightseas 0:633cef71e6ba 74 {
nightseas 0:633cef71e6ba 75 switch(channel)
nightseas 0:633cef71e6ba 76 {
nightseas 0:633cef71e6ba 77 case 1:
nightseas 0:633cef71e6ba 78 return vin_bat.read() * 3.3 / VmonDivision[channel - 1];
nightseas 0:633cef71e6ba 79
nightseas 0:633cef71e6ba 80 case 2:
nightseas 0:633cef71e6ba 81 return vin_12v.read() * 3.3 / VmonDivision[channel - 1];
nightseas 0:633cef71e6ba 82
nightseas 0:633cef71e6ba 83 case 3:
nightseas 0:633cef71e6ba 84 return vin_5v.read() * 3.3 / VmonDivision[channel - 1];
nightseas 0:633cef71e6ba 85
nightseas 0:633cef71e6ba 86 case 4:
nightseas 0:633cef71e6ba 87 return vin_3v3.read() * 3.3 / VmonDivision[channel - 1];
nightseas 0:633cef71e6ba 88
nightseas 0:633cef71e6ba 89 default:
nightseas 0:633cef71e6ba 90 return 0;
nightseas 0:633cef71e6ba 91 }
nightseas 0:633cef71e6ba 92 }
nightseas 0:633cef71e6ba 93
nightseas 0:633cef71e6ba 94
nightseas 0:633cef71e6ba 95 /*********************************************/
nightseas 0:633cef71e6ba 96 /* DC Motor Control Functions */
nightseas 0:633cef71e6ba 97 /*********************************************/
nightseas 0:633cef71e6ba 98 //Init the PWM and direction of motors to STOP state
nightseas 0:633cef71e6ba 99 void Mot_Init(void)
nightseas 0:633cef71e6ba 100 {
nightseas 0:633cef71e6ba 101 for(int motNum = 1; motNum <= MOT_NUM_MAX; motNum++)
nightseas 0:633cef71e6ba 102 {
nightseas 0:633cef71e6ba 103 Mot_PwmSetup(motNum, 1000, 0); //Freq = 1KHz, Speed = 0%
nightseas 0:633cef71e6ba 104 Mot_SetDirection(motNum, 's'); //Direction = STOP
nightseas 0:633cef71e6ba 105 }
nightseas 0:633cef71e6ba 106 }
nightseas 0:633cef71e6ba 107
nightseas 0:633cef71e6ba 108 //Control the movement of vehicle
nightseas 0:633cef71e6ba 109 //dir: forward = 'f', backward = 'b', stop = 's'
nightseas 0:633cef71e6ba 110 //turn: left = 'l', right = 'r', fixed turning rate
nightseas 0:633cef71e6ba 111 //speed: 0.0~1.0 (0~100%, 0.245 = 24.5%)
nightseas 0:633cef71e6ba 112 void Mot_Ctrl(char dir, char turn, float speed)
nightseas 0:633cef71e6ba 113 {
nightseas 0:633cef71e6ba 114 if(speed < 0)
nightseas 0:633cef71e6ba 115 speed = 0;
nightseas 0:633cef71e6ba 116 if(speed > 1)
nightseas 0:633cef71e6ba 117 speed = 1;
nightseas 0:633cef71e6ba 118
nightseas 0:633cef71e6ba 119 switch(turn)
nightseas 0:633cef71e6ba 120 {
nightseas 0:633cef71e6ba 121 case 'l':
nightseas 0:633cef71e6ba 122 Mot_PwmWrite(1, speed / MOT_TURN_RATE);
nightseas 0:633cef71e6ba 123 Mot_SetDirection(1, dir);
nightseas 0:633cef71e6ba 124 Mot_PwmWrite(2, speed);
nightseas 0:633cef71e6ba 125 Mot_SetDirection(2, dir);
nightseas 0:633cef71e6ba 126
nightseas 0:633cef71e6ba 127 if(MOT_NUM_MAX > 2)
nightseas 0:633cef71e6ba 128 {
nightseas 0:633cef71e6ba 129 Mot_PwmWrite(3, speed / MOT_TURN_RATE);
nightseas 0:633cef71e6ba 130 Mot_SetDirection(3, dir);
nightseas 0:633cef71e6ba 131 Mot_PwmWrite(4, speed);
nightseas 0:633cef71e6ba 132 Mot_SetDirection(4, dir);
nightseas 0:633cef71e6ba 133 }
nightseas 0:633cef71e6ba 134 break;
nightseas 0:633cef71e6ba 135
nightseas 0:633cef71e6ba 136 case 'r':
nightseas 0:633cef71e6ba 137 Mot_PwmWrite(1, speed);
nightseas 0:633cef71e6ba 138 Mot_SetDirection(1, dir);
nightseas 0:633cef71e6ba 139 Mot_PwmWrite(2, speed / MOT_TURN_RATE);
nightseas 0:633cef71e6ba 140 Mot_SetDirection(2, dir);
nightseas 0:633cef71e6ba 141
nightseas 0:633cef71e6ba 142 if(MOT_NUM_MAX > 2)
nightseas 0:633cef71e6ba 143 {
nightseas 0:633cef71e6ba 144 Mot_PwmWrite(3, speed);
nightseas 0:633cef71e6ba 145 Mot_SetDirection(3, dir);
nightseas 0:633cef71e6ba 146 Mot_PwmWrite(4, speed / MOT_TURN_RATE);
nightseas 0:633cef71e6ba 147 Mot_SetDirection(4, dir);
nightseas 0:633cef71e6ba 148 }
nightseas 0:633cef71e6ba 149 break;
nightseas 0:633cef71e6ba 150
nightseas 0:633cef71e6ba 151 case 's':
nightseas 0:633cef71e6ba 152 for(int motNum = 1; motNum <= MOT_NUM_MAX; motNum++)
nightseas 0:633cef71e6ba 153 {
nightseas 0:633cef71e6ba 154 Mot_PwmWrite(motNum, 0);
nightseas 0:633cef71e6ba 155 Mot_SetDirection(motNum, 's');
nightseas 0:633cef71e6ba 156 }
nightseas 0:633cef71e6ba 157 break;
nightseas 0:633cef71e6ba 158 }
nightseas 0:633cef71e6ba 159 }
nightseas 0:633cef71e6ba 160
nightseas 0:633cef71e6ba 161 //channel: 1~4, period: us, duty: 0.0~1.0 (0.245 = 24.5%)
nightseas 0:633cef71e6ba 162 void Mot_PwmSetup(int channel, int period, float duty)
nightseas 0:633cef71e6ba 163 {
nightseas 0:633cef71e6ba 164 if(duty < 0)
nightseas 0:633cef71e6ba 165 duty = 0;
nightseas 0:633cef71e6ba 166 else if(duty > 1)
nightseas 0:633cef71e6ba 167 duty = 1;
nightseas 0:633cef71e6ba 168
nightseas 0:633cef71e6ba 169 switch(channel)
nightseas 0:633cef71e6ba 170 {
nightseas 0:633cef71e6ba 171 case 1:
nightseas 0:633cef71e6ba 172 mot1_pwm.period_us(period);
nightseas 0:633cef71e6ba 173 mot1_pwm = duty;
nightseas 0:633cef71e6ba 174 break;
nightseas 0:633cef71e6ba 175 case 2:
nightseas 0:633cef71e6ba 176 mot2_pwm.period_us(period);
nightseas 0:633cef71e6ba 177 mot2_pwm = duty;
nightseas 0:633cef71e6ba 178 break;
nightseas 0:633cef71e6ba 179 case 3:
nightseas 0:633cef71e6ba 180 mot3_pwm.period_us(period);
nightseas 0:633cef71e6ba 181 mot3_pwm = duty;
nightseas 0:633cef71e6ba 182 break;
nightseas 0:633cef71e6ba 183 case 4:
nightseas 0:633cef71e6ba 184 mot4_pwm.period_us(period);
nightseas 0:633cef71e6ba 185 mot4_pwm = duty;
nightseas 0:633cef71e6ba 186 break;
nightseas 0:633cef71e6ba 187
nightseas 0:633cef71e6ba 188 default:
nightseas 0:633cef71e6ba 189 break;
nightseas 0:633cef71e6ba 190 }
nightseas 0:633cef71e6ba 191 }
nightseas 0:633cef71e6ba 192
nightseas 0:633cef71e6ba 193 //channel: 1~4, duty: 0.0~1.0 (0.245 = 24.5%)
nightseas 0:633cef71e6ba 194 void Mot_PwmWrite(int channel, float duty)
nightseas 0:633cef71e6ba 195 {
nightseas 0:633cef71e6ba 196 if(duty < 0)
nightseas 0:633cef71e6ba 197 duty = 0;
nightseas 0:633cef71e6ba 198 else if(duty > 1)
nightseas 0:633cef71e6ba 199 duty = 1;
nightseas 0:633cef71e6ba 200
nightseas 0:633cef71e6ba 201 switch(channel)
nightseas 0:633cef71e6ba 202 {
nightseas 0:633cef71e6ba 203 //All channels in TIM3 share the same period
nightseas 0:633cef71e6ba 204 case 1:
nightseas 0:633cef71e6ba 205 mot1_pwm = duty;
nightseas 0:633cef71e6ba 206 break;
nightseas 0:633cef71e6ba 207 case 2:
nightseas 0:633cef71e6ba 208 mot2_pwm = duty;
nightseas 0:633cef71e6ba 209 break;
nightseas 0:633cef71e6ba 210 case 3:
nightseas 0:633cef71e6ba 211 mot3_pwm = duty;
nightseas 0:633cef71e6ba 212 break;
nightseas 0:633cef71e6ba 213 case 4:
nightseas 0:633cef71e6ba 214 mot4_pwm = duty;
nightseas 0:633cef71e6ba 215 break;
nightseas 0:633cef71e6ba 216
nightseas 0:633cef71e6ba 217 default:
nightseas 0:633cef71e6ba 218 break;
nightseas 0:633cef71e6ba 219 }
nightseas 0:633cef71e6ba 220 }
nightseas 0:633cef71e6ba 221
nightseas 0:633cef71e6ba 222 //channel: 1~4, dir: forward = 'f', backward = 'b', stop = 's'
nightseas 0:633cef71e6ba 223 void Mot_SetDirection(int channel, char dir)
nightseas 0:633cef71e6ba 224 {
nightseas 0:633cef71e6ba 225 switch(channel)
nightseas 0:633cef71e6ba 226 {
nightseas 0:633cef71e6ba 227 case 1:
nightseas 0:633cef71e6ba 228 if(dir == 'f')
nightseas 0:633cef71e6ba 229 mot1_dir = 0x1;
nightseas 0:633cef71e6ba 230 else if(dir == 'b')
nightseas 0:633cef71e6ba 231 mot1_dir = 0x2;
nightseas 0:633cef71e6ba 232 else
nightseas 0:633cef71e6ba 233 mot1_dir = 0x0;
nightseas 0:633cef71e6ba 234 break;
nightseas 0:633cef71e6ba 235 case 2:
nightseas 0:633cef71e6ba 236 if(dir == 'f')
nightseas 0:633cef71e6ba 237 mot2_dir = 0x1;
nightseas 0:633cef71e6ba 238 else if(dir == 'b')
nightseas 0:633cef71e6ba 239 mot2_dir = 0x2;
nightseas 0:633cef71e6ba 240 else
nightseas 0:633cef71e6ba 241 mot2_dir = 0x0;
nightseas 0:633cef71e6ba 242 break;
nightseas 0:633cef71e6ba 243 case 3:
nightseas 0:633cef71e6ba 244 if(dir == 'f')
nightseas 0:633cef71e6ba 245 mot3_dir = 0x1;
nightseas 0:633cef71e6ba 246 else if(dir == 'b')
nightseas 0:633cef71e6ba 247 mot3_dir = 0x2;
nightseas 0:633cef71e6ba 248 else
nightseas 0:633cef71e6ba 249 mot3_dir = 0x0;
nightseas 0:633cef71e6ba 250 break;
nightseas 0:633cef71e6ba 251 case 4:
nightseas 0:633cef71e6ba 252 if(dir == 'f')
nightseas 0:633cef71e6ba 253 mot4_dir = 0x1;
nightseas 0:633cef71e6ba 254 else if(dir == 'b')
nightseas 0:633cef71e6ba 255 mot4_dir = 0x2;
nightseas 0:633cef71e6ba 256 else
nightseas 0:633cef71e6ba 257 mot4_dir = 0x0;
nightseas 0:633cef71e6ba 258 break;
nightseas 0:633cef71e6ba 259 }
nightseas 0:633cef71e6ba 260 }
nightseas 0:633cef71e6ba 261
nightseas 0:633cef71e6ba 262 /*********************************************/
nightseas 0:633cef71e6ba 263 /* Servo Control Functions */
nightseas 0:633cef71e6ba 264 /*********************************************/
nightseas 0:633cef71e6ba 265 //Init the PWM and direction of motors to STOP state
nightseas 0:633cef71e6ba 266 void Servo_Init(void)
nightseas 0:633cef71e6ba 267 {
nightseas 0:633cef71e6ba 268 for(int servoNum = 1; servoNum <= SRV_NUM_MAX; servoNum++)
nightseas 0:633cef71e6ba 269 {
nightseas 0:633cef71e6ba 270 Servo_SetAngle(servoNum, 0);
nightseas 0:633cef71e6ba 271 }
nightseas 0:633cef71e6ba 272 }
nightseas 0:633cef71e6ba 273
nightseas 0:633cef71e6ba 274 //channel: 1~4, angle: -90.0~+90.0
nightseas 0:633cef71e6ba 275 void Servo_SetAngle(int channel, float angle)
nightseas 0:633cef71e6ba 276 {
nightseas 0:633cef71e6ba 277 if(channel > 0 && channel <= SRV_NUM_MAX)
nightseas 0:633cef71e6ba 278 {
nightseas 0:633cef71e6ba 279 int pulse = (int)((angle + 90.0) * (ServoCal[channel - 1][1] - ServoCal[channel-1][0]) / 180.0 + ServoCal[channel-1][0]);
nightseas 0:633cef71e6ba 280 Servo_PwmWrite(channel, pulse);
nightseas 0:633cef71e6ba 281 }
nightseas 0:633cef71e6ba 282 }
nightseas 0:633cef71e6ba 283
nightseas 0:633cef71e6ba 284 //channel: 1~4, pulse_us: 0~20000us
nightseas 0:633cef71e6ba 285 void Servo_PwmSetup(int channel, int pulse_us)
nightseas 0:633cef71e6ba 286 {
nightseas 0:633cef71e6ba 287 if(pulse_us < 0)
nightseas 0:633cef71e6ba 288 pulse_us = 0;
nightseas 0:633cef71e6ba 289 else if(pulse_us > 20000)
nightseas 0:633cef71e6ba 290 pulse_us = 20000;
nightseas 0:633cef71e6ba 291
nightseas 0:633cef71e6ba 292 switch(channel)
nightseas 0:633cef71e6ba 293 {
nightseas 0:633cef71e6ba 294 case 1:
nightseas 0:633cef71e6ba 295 servo1_pwm.period_us(SRV_PERIOD_US);
nightseas 0:633cef71e6ba 296 servo1_pwm.pulsewidth_us(pulse_us);
nightseas 0:633cef71e6ba 297 break;
nightseas 0:633cef71e6ba 298 case 2:
nightseas 0:633cef71e6ba 299 servo2_pwm.period_us(SRV_PERIOD_US);
nightseas 0:633cef71e6ba 300 servo2_pwm.pulsewidth_us(pulse_us);
nightseas 0:633cef71e6ba 301 break;
nightseas 0:633cef71e6ba 302 case 3:
nightseas 0:633cef71e6ba 303 servo3_pwm.period_us(SRV_PERIOD_US);
nightseas 0:633cef71e6ba 304 servo3_pwm.pulsewidth_us(pulse_us);
nightseas 0:633cef71e6ba 305 break;
nightseas 0:633cef71e6ba 306 case 4:
nightseas 0:633cef71e6ba 307 servo4_pwm.period_us(SRV_PERIOD_US);
nightseas 0:633cef71e6ba 308 servo4_pwm.pulsewidth_us(pulse_us);
nightseas 0:633cef71e6ba 309 break;
nightseas 0:633cef71e6ba 310
nightseas 0:633cef71e6ba 311 default:
nightseas 0:633cef71e6ba 312 break;
nightseas 0:633cef71e6ba 313 }
nightseas 0:633cef71e6ba 314 }
nightseas 0:633cef71e6ba 315
nightseas 0:633cef71e6ba 316 //channel: 1~4, pulse_us: 0~20000us
nightseas 0:633cef71e6ba 317 void Servo_PwmWrite(int channel, int pulse_us)
nightseas 0:633cef71e6ba 318 {
nightseas 0:633cef71e6ba 319 if(pulse_us < 0)
nightseas 0:633cef71e6ba 320 pulse_us = 0;
nightseas 0:633cef71e6ba 321 else if(pulse_us > 20000)
nightseas 0:633cef71e6ba 322 pulse_us = 20000;
nightseas 0:633cef71e6ba 323
nightseas 0:633cef71e6ba 324 switch(channel)
nightseas 0:633cef71e6ba 325 {
nightseas 0:633cef71e6ba 326 //All channels in TIM3 share the same period
nightseas 0:633cef71e6ba 327 case 1:
nightseas 0:633cef71e6ba 328 servo1_pwm.pulsewidth_us(pulse_us);
nightseas 0:633cef71e6ba 329 break;
nightseas 0:633cef71e6ba 330 case 2:
nightseas 0:633cef71e6ba 331 servo2_pwm.pulsewidth_us(pulse_us);
nightseas 0:633cef71e6ba 332 break;
nightseas 0:633cef71e6ba 333 case 3:
nightseas 0:633cef71e6ba 334 servo3_pwm.pulsewidth_us(pulse_us);
nightseas 0:633cef71e6ba 335 break;
nightseas 0:633cef71e6ba 336 case 4:
nightseas 0:633cef71e6ba 337 servo4_pwm.pulsewidth_us(pulse_us);
nightseas 0:633cef71e6ba 338 break;
nightseas 0:633cef71e6ba 339
nightseas 0:633cef71e6ba 340 default:
nightseas 0:633cef71e6ba 341 break;
nightseas 0:633cef71e6ba 342 }
nightseas 0:633cef71e6ba 343 }
nightseas 0:633cef71e6ba 344
nightseas 0:633cef71e6ba 345 /*********************************************/
nightseas 0:633cef71e6ba 346 /* LED Control Functions */
nightseas 0:633cef71e6ba 347 /*********************************************/
nightseas 0:633cef71e6ba 348 void LedOn(int ch)
nightseas 0:633cef71e6ba 349 {
nightseas 0:633cef71e6ba 350 switch(ch)
nightseas 0:633cef71e6ba 351 {
nightseas 0:633cef71e6ba 352 case 0:
nightseas 0:633cef71e6ba 353 led_mb = 1;
nightseas 0:633cef71e6ba 354 break;
nightseas 0:633cef71e6ba 355
nightseas 0:633cef71e6ba 356 default:
nightseas 0:633cef71e6ba 357 break;
nightseas 0:633cef71e6ba 358 }
nightseas 0:633cef71e6ba 359 }
nightseas 0:633cef71e6ba 360
nightseas 0:633cef71e6ba 361 void LedOff(int ch)
nightseas 0:633cef71e6ba 362 {
nightseas 0:633cef71e6ba 363 switch(ch)
nightseas 0:633cef71e6ba 364 {
nightseas 0:633cef71e6ba 365 case 0:
nightseas 0:633cef71e6ba 366 led_mb = 0;
nightseas 0:633cef71e6ba 367 break;
nightseas 0:633cef71e6ba 368
nightseas 0:633cef71e6ba 369 default:
nightseas 0:633cef71e6ba 370 break;
nightseas 0:633cef71e6ba 371 }
nightseas 0:633cef71e6ba 372 }
nightseas 0:633cef71e6ba 373
nightseas 0:633cef71e6ba 374 void LedToggle(int ch)
nightseas 0:633cef71e6ba 375 {
nightseas 0:633cef71e6ba 376 switch(ch)
nightseas 0:633cef71e6ba 377 {
nightseas 0:633cef71e6ba 378 case 0:
nightseas 0:633cef71e6ba 379 led_mb = !led_mb;
nightseas 0:633cef71e6ba 380 break;
nightseas 0:633cef71e6ba 381
nightseas 0:633cef71e6ba 382 default:
nightseas 0:633cef71e6ba 383 break;
nightseas 0:633cef71e6ba 384 }
nightseas 0:633cef71e6ba 385 }
nightseas 0:633cef71e6ba 386
nightseas 0:633cef71e6ba 387 void LedOnAll(void)
nightseas 0:633cef71e6ba 388 {
nightseas 0:633cef71e6ba 389 for(int ledNum = 0; ledNum < LED_NUM_MAX; ledNum++)
nightseas 0:633cef71e6ba 390 LedOn(ledNum);
nightseas 0:633cef71e6ba 391 }
nightseas 0:633cef71e6ba 392
nightseas 0:633cef71e6ba 393 void LedOffAll(void)
nightseas 0:633cef71e6ba 394 {
nightseas 0:633cef71e6ba 395 for(int ledNum = 0; ledNum < LED_NUM_MAX; ledNum++)
nightseas 0:633cef71e6ba 396 LedOff(ledNum);
nightseas 0:633cef71e6ba 397 }