Gilisymo Hitech / Mbed 2 deprecated Nucleo401_Radar_LS53L0X

// 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

Dependencies:   mbed

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?

UserRevisionLine numberNew 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