// copright gilisymo www.gilisymo.com //This program has been demonstrated at Grenoble makerfaire 2017 by the gilisymo team //This program reads the PWM output of the LS53L0X laser distance sensor from Gilisymo using an STMicroelectronics nucleo F401 board //it controls a servo from hobby king (HK15178 https://hobbyking.com/fr_fr/hobbykingtm-hk15178-analog-servo-1-4kg-0-09sec-10g.html) //the measured distance is send to UART, for interface with HC06 bluetooth serial SPP controler //Developer: Sylvain TRIVIAUX
main.cpp@1:682853d16f3a, 2017-04-02 (annotated)
- Committer:
- gilisymo
- Date:
- Sun Apr 02 20:21:12 2017 +0000
- Revision:
- 1:682853d16f3a
- Parent:
- 0:048cd12ff21c
typo
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
gilisymo | 0:048cd12ff21c | 1 | #include "mbed.h" |
gilisymo | 0:048cd12ff21c | 2 | #include "math.h" |
gilisymo | 0:048cd12ff21c | 3 | |
gilisymo | 1:682853d16f3a | 4 | // copyright gilisymo www.gilisymo.com |
gilisymo | 0:048cd12ff21c | 5 | //This program has been demonstrated at Grenoble makerfaire 2017 by the gilisymo team |
gilisymo | 0:048cd12ff21c | 6 | |
gilisymo | 0:048cd12ff21c | 7 | //This program reads the PWM output of the LS53L0X laser distance sensor from Gilisymo using an STMicroelectronics nucleo F401 board |
gilisymo | 0:048cd12ff21c | 8 | //it controls a servo from hobby king (HK15178 https://hobbyking.com/fr_fr/hobbykingtm-hk15178-analog-servo-1-4kg-0-09sec-10g.html) |
gilisymo | 0:048cd12ff21c | 9 | //the measured distance is send to UART, for interface with HC06 bluetooth serial SPP controler |
gilisymo | 0:048cd12ff21c | 10 | |
gilisymo | 0:048cd12ff21c | 11 | //Developer: Sylvain TRIVIAUX |
gilisymo | 0:048cd12ff21c | 12 | |
gilisymo | 0:048cd12ff21c | 13 | PwmOut mypwm(PA_8); |
gilisymo | 0:048cd12ff21c | 14 | PwmOut servo(PB_8); |
gilisymo | 0:048cd12ff21c | 15 | Serial myUART(PA_9, PA_10); |
gilisymo | 0:048cd12ff21c | 16 | DigitalOut myled(LED1); |
gilisymo | 0:048cd12ff21c | 17 | DigitalIn myIOs(PA_0); |
gilisymo | 0:048cd12ff21c | 18 | InterruptIn EWOK_INTR(PA_0); |
gilisymo | 0:048cd12ff21c | 19 | |
gilisymo | 0:048cd12ff21c | 20 | |
gilisymo | 0:048cd12ff21c | 21 | |
gilisymo | 0:048cd12ff21c | 22 | Serial device(PA_2,PA_3); |
gilisymo | 0:048cd12ff21c | 23 | |
gilisymo | 0:048cd12ff21c | 24 | Ticker clockServo; |
gilisymo | 0:048cd12ff21c | 25 | Ticker clockDistance; |
gilisymo | 0:048cd12ff21c | 26 | |
gilisymo | 0:048cd12ff21c | 27 | int counter; |
gilisymo | 0:048cd12ff21c | 28 | int counterDist = 0; |
gilisymo | 0:048cd12ff21c | 29 | |
gilisymo | 0:048cd12ff21c | 30 | int incrPWM =10; |
gilisymo | 0:048cd12ff21c | 31 | //int pw_def = 500; |
gilisymo | 0:048cd12ff21c | 32 | int pw_def ; |
gilisymo | 0:048cd12ff21c | 33 | int pw = 100; |
gilisymo | 0:048cd12ff21c | 34 | int increase=1; |
gilisymo | 0:048cd12ff21c | 35 | |
gilisymo | 0:048cd12ff21c | 36 | float TICK_FREQ_SERVO = 0.01; // 10ms |
gilisymo | 0:048cd12ff21c | 37 | float TICK_FREQ_DIST = 0.0001; //0.1us |
gilisymo | 0:048cd12ff21c | 38 | |
gilisymo | 0:048cd12ff21c | 39 | //CONFIG SERVO |
gilisymo | 0:048cd12ff21c | 40 | int SERVO_MAX = 2500; |
gilisymo | 0:048cd12ff21c | 41 | int SERVO_MIN = 500; |
gilisymo | 0:048cd12ff21c | 42 | |
gilisymo | 0:048cd12ff21c | 43 | int SERVO_PERIOD_MS = 10; |
gilisymo | 0:048cd12ff21c | 44 | int SERVO_PW_DEF_US = 500; |
gilisymo | 0:048cd12ff21c | 45 | |
gilisymo | 0:048cd12ff21c | 46 | |
gilisymo | 0:048cd12ff21c | 47 | int UART_BAUDS_DEF = 9600; |
gilisymo | 0:048cd12ff21c | 48 | |
gilisymo | 0:048cd12ff21c | 49 | int angle_servo = 0; |
gilisymo | 0:048cd12ff21c | 50 | int direction = 0; |
gilisymo | 0:048cd12ff21c | 51 | |
gilisymo | 0:048cd12ff21c | 52 | void initServo(PwmOut* servo, int servo_period_ms, int pw_def_us) |
gilisymo | 0:048cd12ff21c | 53 | { |
gilisymo | 0:048cd12ff21c | 54 | printf("\t\tSERVO INITIALIZATION START\n"); |
gilisymo | 0:048cd12ff21c | 55 | servo->period_ms(servo_period_ms); |
gilisymo | 0:048cd12ff21c | 56 | servo->pulsewidth_us(pw_def_us); //butee droite |
gilisymo | 0:048cd12ff21c | 57 | //pw = pw_def_us; |
gilisymo | 0:048cd12ff21c | 58 | printf("\t\tSERVO INITIALIZATION DONE\n"); |
gilisymo | 0:048cd12ff21c | 59 | } |
gilisymo | 0:048cd12ff21c | 60 | |
gilisymo | 0:048cd12ff21c | 61 | void positionServo(PwmOut* servo, int angle_deg) |
gilisymo | 0:048cd12ff21c | 62 | { |
gilisymo | 0:048cd12ff21c | 63 | int pw_angle; |
gilisymo | 0:048cd12ff21c | 64 | float ratio = (SERVO_MAX - SERVO_MIN)/180; |
gilisymo | 0:048cd12ff21c | 65 | pw_angle = (int) (angle_deg*ratio+SERVO_MIN); |
gilisymo | 0:048cd12ff21c | 66 | servo->pulsewidth_us( pw_angle); |
gilisymo | 0:048cd12ff21c | 67 | } |
gilisymo | 0:048cd12ff21c | 68 | |
gilisymo | 0:048cd12ff21c | 69 | //Interruption |
gilisymo | 0:048cd12ff21c | 70 | |
gilisymo | 0:048cd12ff21c | 71 | void clockServo_int() |
gilisymo | 0:048cd12ff21c | 72 | { |
gilisymo | 0:048cd12ff21c | 73 | int angle; |
gilisymo | 0:048cd12ff21c | 74 | |
gilisymo | 0:048cd12ff21c | 75 | switch (direction) |
gilisymo | 0:048cd12ff21c | 76 | { |
gilisymo | 0:048cd12ff21c | 77 | case 0: |
gilisymo | 0:048cd12ff21c | 78 | { |
gilisymo | 0:048cd12ff21c | 79 | //printf("case 0\n;"); |
gilisymo | 0:048cd12ff21c | 80 | if(angle_servo >180) |
gilisymo | 0:048cd12ff21c | 81 | { |
gilisymo | 0:048cd12ff21c | 82 | direction = 1; |
gilisymo | 0:048cd12ff21c | 83 | angle_servo= angle_servo -1; |
gilisymo | 0:048cd12ff21c | 84 | break; |
gilisymo | 0:048cd12ff21c | 85 | } |
gilisymo | 0:048cd12ff21c | 86 | |
gilisymo | 0:048cd12ff21c | 87 | if(angle_servo < 0) |
gilisymo | 0:048cd12ff21c | 88 | { |
gilisymo | 0:048cd12ff21c | 89 | direction = 0; |
gilisymo | 0:048cd12ff21c | 90 | angle_servo= angle_servo +1; |
gilisymo | 0:048cd12ff21c | 91 | break; |
gilisymo | 0:048cd12ff21c | 92 | } |
gilisymo | 0:048cd12ff21c | 93 | angle_servo= angle_servo +1; |
gilisymo | 0:048cd12ff21c | 94 | break; |
gilisymo | 0:048cd12ff21c | 95 | } |
gilisymo | 0:048cd12ff21c | 96 | |
gilisymo | 0:048cd12ff21c | 97 | case 1: |
gilisymo | 0:048cd12ff21c | 98 | { |
gilisymo | 0:048cd12ff21c | 99 | |
gilisymo | 0:048cd12ff21c | 100 | //printf("case 1\n;"); |
gilisymo | 0:048cd12ff21c | 101 | if(angle_servo >=180) |
gilisymo | 0:048cd12ff21c | 102 | { |
gilisymo | 0:048cd12ff21c | 103 | direction = 1; |
gilisymo | 0:048cd12ff21c | 104 | angle_servo= angle_servo -1; |
gilisymo | 0:048cd12ff21c | 105 | break; |
gilisymo | 0:048cd12ff21c | 106 | } |
gilisymo | 0:048cd12ff21c | 107 | |
gilisymo | 0:048cd12ff21c | 108 | if(angle_servo <= 0) |
gilisymo | 0:048cd12ff21c | 109 | { |
gilisymo | 0:048cd12ff21c | 110 | direction = 0; |
gilisymo | 0:048cd12ff21c | 111 | angle_servo= angle_servo +1; |
gilisymo | 0:048cd12ff21c | 112 | break; |
gilisymo | 0:048cd12ff21c | 113 | } |
gilisymo | 0:048cd12ff21c | 114 | angle_servo = angle_servo -1; |
gilisymo | 0:048cd12ff21c | 115 | break; |
gilisymo | 0:048cd12ff21c | 116 | } |
gilisymo | 0:048cd12ff21c | 117 | |
gilisymo | 0:048cd12ff21c | 118 | |
gilisymo | 0:048cd12ff21c | 119 | |
gilisymo | 0:048cd12ff21c | 120 | } |
gilisymo | 0:048cd12ff21c | 121 | //printf("angle_servo %i\n", angle_servo); |
gilisymo | 0:048cd12ff21c | 122 | positionServo(&servo, angle_servo); |
gilisymo | 0:048cd12ff21c | 123 | |
gilisymo | 0:048cd12ff21c | 124 | }//void clock10ms_int |
gilisymo | 0:048cd12ff21c | 125 | |
gilisymo | 0:048cd12ff21c | 126 | |
gilisymo | 0:048cd12ff21c | 127 | void clockDist_int() |
gilisymo | 0:048cd12ff21c | 128 | { |
gilisymo | 0:048cd12ff21c | 129 | counterDist = counterDist +1; |
gilisymo | 0:048cd12ff21c | 130 | } |
gilisymo | 0:048cd12ff21c | 131 | |
gilisymo | 0:048cd12ff21c | 132 | |
gilisymo | 0:048cd12ff21c | 133 | char* PACKET_START= "PACKET_START"; |
gilisymo | 0:048cd12ff21c | 134 | char* PACKET_END= "PACKET_END"; |
gilisymo | 0:048cd12ff21c | 135 | |
gilisymo | 0:048cd12ff21c | 136 | char* SERVO_POS_START= "SERVO_POS_START"; |
gilisymo | 0:048cd12ff21c | 137 | char* SERVO_POS_END= "SERVO_POS_END"; |
gilisymo | 0:048cd12ff21c | 138 | |
gilisymo | 0:048cd12ff21c | 139 | |
gilisymo | 0:048cd12ff21c | 140 | |
gilisymo | 0:048cd12ff21c | 141 | time_t interuptionTime = time(NULL); |
gilisymo | 0:048cd12ff21c | 142 | |
gilisymo | 0:048cd12ff21c | 143 | time_t interUp ; |
gilisymo | 0:048cd12ff21c | 144 | time_t interDown; |
gilisymo | 0:048cd12ff21c | 145 | int duration ; |
gilisymo | 0:048cd12ff21c | 146 | Timer EWOK; |
gilisymo | 0:048cd12ff21c | 147 | |
gilisymo | 0:048cd12ff21c | 148 | |
gilisymo | 0:048cd12ff21c | 149 | |
gilisymo | 0:048cd12ff21c | 150 | void EWOK_UP() |
gilisymo | 0:048cd12ff21c | 151 | { |
gilisymo | 0:048cd12ff21c | 152 | EWOK.start(); |
gilisymo | 0:048cd12ff21c | 153 | interUp = time(&interuptionTime); |
gilisymo | 0:048cd12ff21c | 154 | } |
gilisymo | 0:048cd12ff21c | 155 | |
gilisymo | 0:048cd12ff21c | 156 | void EWOK_DOWN() |
gilisymo | 0:048cd12ff21c | 157 | { |
gilisymo | 0:048cd12ff21c | 158 | EWOK.stop(); |
gilisymo | 0:048cd12ff21c | 159 | duration =EWOK.read_us(); |
gilisymo | 0:048cd12ff21c | 160 | EWOK.reset(); |
gilisymo | 0:048cd12ff21c | 161 | printf("duration %i\n", duration); |
gilisymo | 0:048cd12ff21c | 162 | |
gilisymo | 0:048cd12ff21c | 163 | } |
gilisymo | 0:048cd12ff21c | 164 | |
gilisymo | 0:048cd12ff21c | 165 | |
gilisymo | 0:048cd12ff21c | 166 | |
gilisymo | 0:048cd12ff21c | 167 | int main() { |
gilisymo | 0:048cd12ff21c | 168 | printf("\n\n\n##### Nucleo RADAR ######\n"); |
gilisymo | 0:048cd12ff21c | 169 | printf("\n\nRadar Initialization\n"); |
gilisymo | 0:048cd12ff21c | 170 | |
gilisymo | 0:048cd12ff21c | 171 | myled = 0; |
gilisymo | 0:048cd12ff21c | 172 | counter = 0; |
gilisymo | 0:048cd12ff21c | 173 | pw_def = SERVO_PW_DEF_US; |
gilisymo | 0:048cd12ff21c | 174 | pw = SERVO_PW_DEF_US; |
gilisymo | 0:048cd12ff21c | 175 | myUART.baud(UART_BAUDS_DEF); |
gilisymo | 0:048cd12ff21c | 176 | clockServo.attach(&clockServo_int, TICK_FREQ_SERVO); |
gilisymo | 0:048cd12ff21c | 177 | |
gilisymo | 0:048cd12ff21c | 178 | mypwm.period_ms(10); |
gilisymo | 0:048cd12ff21c | 179 | mypwm.pulsewidth_ms(1); |
gilisymo | 0:048cd12ff21c | 180 | initServo(&servo, SERVO_PERIOD_MS, SERVO_PW_DEF_US); |
gilisymo | 0:048cd12ff21c | 181 | printf("\nRadar Initialization done\n"); |
gilisymo | 0:048cd12ff21c | 182 | int angle =0; |
gilisymo | 0:048cd12ff21c | 183 | EWOK.reset(); |
gilisymo | 0:048cd12ff21c | 184 | EWOK.stop(); |
gilisymo | 0:048cd12ff21c | 185 | EWOK_INTR.rise(&EWOK_UP); |
gilisymo | 0:048cd12ff21c | 186 | EWOK_INTR.fall(&EWOK_DOWN); |
gilisymo | 0:048cd12ff21c | 187 | |
gilisymo | 0:048cd12ff21c | 188 | while(1){ |
gilisymo | 0:048cd12ff21c | 189 | |
gilisymo | 0:048cd12ff21c | 190 | //manual command of servo through terminal |
gilisymo | 0:048cd12ff21c | 191 | printf("Use following keys\n"); |
gilisymo | 0:048cd12ff21c | 192 | printf("\tEnter to start Radar\n"); |
gilisymo | 0:048cd12ff21c | 193 | printf("\tt: to center the Radar\n"); |
gilisymo | 0:048cd12ff21c | 194 | printf("\ty to set it to the right\n"); |
gilisymo | 0:048cd12ff21c | 195 | printf("\tr to set it to the left\n"); |
gilisymo | 0:048cd12ff21c | 196 | |
gilisymo | 0:048cd12ff21c | 197 | /* |
gilisymo | 0:048cd12ff21c | 198 | char c = device.getc(); |
gilisymo | 0:048cd12ff21c | 199 | if(c == 'r') { //r-> left |
gilisymo | 0:048cd12ff21c | 200 | servo.pulsewidth_us(2500); |
gilisymo | 0:048cd12ff21c | 201 | } |
gilisymo | 0:048cd12ff21c | 202 | if(c == 'y') { //y-> right |
gilisymo | 0:048cd12ff21c | 203 | servo.pulsewidth_us(500); |
gilisymo | 0:048cd12ff21c | 204 | } |
gilisymo | 0:048cd12ff21c | 205 | |
gilisymo | 0:048cd12ff21c | 206 | if(c == 't') { //t-> center |
gilisymo | 0:048cd12ff21c | 207 | servo.pulsewidth_us(1500); |
gilisymo | 0:048cd12ff21c | 208 | } |
gilisymo | 0:048cd12ff21c | 209 | |
gilisymo | 0:048cd12ff21c | 210 | if(c == '\r') { //enter = launching radar |
gilisymo | 0:048cd12ff21c | 211 | */ |
gilisymo | 0:048cd12ff21c | 212 | |
gilisymo | 0:048cd12ff21c | 213 | printf("Enter pressed, starting radar\n"); |
gilisymo | 0:048cd12ff21c | 214 | while(1) |
gilisymo | 0:048cd12ff21c | 215 | { |
gilisymo | 0:048cd12ff21c | 216 | //printf("%i:%i\n", angle_servo, duration); |
gilisymo | 0:048cd12ff21c | 217 | myUART.printf("%i:%i\n", angle_servo, duration); |
gilisymo | 0:048cd12ff21c | 218 | } |
gilisymo | 0:048cd12ff21c | 219 | |
gilisymo | 0:048cd12ff21c | 220 | /* |
gilisymo | 0:048cd12ff21c | 221 | if(c == 'r') { //r-> left |
gilisymo | 0:048cd12ff21c | 222 | manualServo = 0; |
gilisymo | 0:048cd12ff21c | 223 | manualServo = 1; |
gilisymo | 0:048cd12ff21c | 224 | wait_us(2500); |
gilisymo | 0:048cd12ff21c | 225 | manualServo = 0; |
gilisymo | 0:048cd12ff21c | 226 | wait_us(1); |
gilisymo | 0:048cd12ff21c | 227 | } |
gilisymo | 0:048cd12ff21c | 228 | |
gilisymo | 0:048cd12ff21c | 229 | if(c == 't') { // t-> right |
gilisymo | 0:048cd12ff21c | 230 | manualServo = 0; |
gilisymo | 0:048cd12ff21c | 231 | manualServo = 1; |
gilisymo | 0:048cd12ff21c | 232 | wait_us(500); |
gilisymo | 0:048cd12ff21c | 233 | manualServo = 0; |
gilisymo | 0:048cd12ff21c | 234 | wait_us(1); |
gilisymo | 0:048cd12ff21c | 235 | } |
gilisymo | 0:048cd12ff21c | 236 | |
gilisymo | 0:048cd12ff21c | 237 | if(c == 'y') { |
gilisymo | 0:048cd12ff21c | 238 | manualServo = 0; |
gilisymo | 0:048cd12ff21c | 239 | //wait_us(10); |
gilisymo | 0:048cd12ff21c | 240 | manualServo = 1; |
gilisymo | 0:048cd12ff21c | 241 | wait_us(1500); |
gilisymo | 0:048cd12ff21c | 242 | manualServo = 0; |
gilisymo | 0:048cd12ff21c | 243 | wait_us(1); |
gilisymo | 0:048cd12ff21c | 244 | //wait_us(49500) ; |
gilisymo | 0:048cd12ff21c | 245 | } |
gilisymo | 0:048cd12ff21c | 246 | */ |
gilisymo | 0:048cd12ff21c | 247 | } |
gilisymo | 0:048cd12ff21c | 248 | }//main |