main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 
00004 // copyright gilisymo www.gilisymo.com
00005 //This program has been demonstrated at Grenoble makerfaire 2017 by the gilisymo team
00006 
00007 //This program reads the PWM output of the LS53L0X laser distance sensor from Gilisymo using an STMicroelectronics nucleo F401 board
00008 //it controls a servo from hobby king (HK15178 https://hobbyking.com/fr_fr/hobbykingtm-hk15178-analog-servo-1-4kg-0-09sec-10g.html)
00009 //the measured distance is send to UART, for interface with HC06 bluetooth serial SPP controler 
00010 
00011 //Developer: Sylvain TRIVIAUX
00012 
00013 PwmOut mypwm(PA_8);
00014 PwmOut servo(PB_8);
00015 Serial myUART(PA_9, PA_10);
00016 DigitalOut myled(LED1);
00017 DigitalIn myIOs(PA_0);
00018 InterruptIn EWOK_INTR(PA_0);
00019 
00020 
00021 
00022 Serial device(PA_2,PA_3);
00023 
00024 Ticker clockServo;
00025 Ticker clockDistance;
00026 
00027 int counter;
00028 int counterDist = 0;
00029 
00030 int incrPWM =10;
00031 //int pw_def = 500;
00032 int pw_def ;
00033 int pw = 100;
00034 int increase=1; 
00035 
00036 float TICK_FREQ_SERVO = 0.01; // 10ms
00037 float TICK_FREQ_DIST = 0.0001; //0.1us
00038 
00039 //CONFIG SERVO
00040 int SERVO_MAX = 2500;
00041 int SERVO_MIN = 500;
00042 
00043 int SERVO_PERIOD_MS = 10;
00044 int SERVO_PW_DEF_US = 500;
00045 
00046 
00047 int UART_BAUDS_DEF = 9600;
00048 
00049 int angle_servo = 0;
00050 int direction = 0;
00051 
00052 void initServo(PwmOut* servo, int servo_period_ms, int pw_def_us)
00053 {
00054     printf("\t\tSERVO INITIALIZATION START\n");
00055     servo->period_ms(servo_period_ms);   
00056     servo->pulsewidth_us(pw_def_us); //butee droite
00057     //pw = pw_def_us;
00058     printf("\t\tSERVO INITIALIZATION DONE\n");
00059 }
00060 
00061 void positionServo(PwmOut* servo, int angle_deg)
00062 {
00063     int pw_angle;
00064     float ratio = (SERVO_MAX - SERVO_MIN)/180;
00065     pw_angle = (int) (angle_deg*ratio+SERVO_MIN);
00066     servo->pulsewidth_us( pw_angle);
00067 }
00068 
00069 //Interruption
00070 
00071 void clockServo_int()
00072 {
00073 int angle;
00074 
00075  switch (direction)
00076  {
00077      case 0:
00078      {
00079         //printf("case 0\n;");
00080          if(angle_servo >180)
00081          {
00082             direction = 1;   
00083             angle_servo= angle_servo -1;         
00084              break;
00085          }
00086          
00087          if(angle_servo < 0)
00088          {
00089             direction = 0; 
00090             angle_servo= angle_servo +1;
00091              break;
00092          }
00093          angle_servo= angle_servo +1;
00094          break;
00095      }
00096      
00097      case 1:
00098      {
00099          
00100          //printf("case 1\n;");
00101          if(angle_servo >=180)
00102          {
00103             direction = 1; 
00104             angle_servo= angle_servo -1;
00105              break;
00106          }
00107          
00108          if(angle_servo <= 0)
00109          {
00110             direction = 0; 
00111             angle_servo= angle_servo +1;
00112              break;
00113          }
00114          angle_servo = angle_servo -1;
00115          break;
00116      }
00117      
00118       
00119      
00120  }
00121    //printf("angle_servo %i\n", angle_servo);
00122    positionServo(&servo, angle_servo);
00123          
00124 }//void clock10ms_int
00125 
00126 
00127 void clockDist_int()
00128 {
00129   counterDist = counterDist +1;    
00130 }
00131 
00132 
00133 char* PACKET_START= "PACKET_START";
00134 char* PACKET_END= "PACKET_END";
00135 
00136 char* SERVO_POS_START= "SERVO_POS_START";
00137 char* SERVO_POS_END= "SERVO_POS_END";
00138 
00139 
00140 
00141 time_t interuptionTime = time(NULL);
00142 
00143   time_t interUp ;
00144     time_t interDown;
00145     int duration ;
00146     Timer EWOK;
00147    
00148     
00149     
00150 void EWOK_UP()
00151 {
00152     EWOK.start();
00153     interUp =   time(&interuptionTime);
00154 }
00155 
00156 void EWOK_DOWN()
00157 {
00158     EWOK.stop();
00159     duration =EWOK.read_us();
00160     EWOK.reset();   
00161     printf("duration %i\n", duration);
00162 
00163 }
00164 
00165 
00166 
00167 int main() {
00168     printf("\n\n\n##### Nucleo RADAR ######\n");
00169     printf("\n\nRadar Initialization\n");
00170     
00171     myled = 0; 
00172     counter = 0;
00173     pw_def = SERVO_PW_DEF_US;
00174     pw = SERVO_PW_DEF_US;
00175     myUART.baud(UART_BAUDS_DEF);   
00176     clockServo.attach(&clockServo_int, TICK_FREQ_SERVO);        
00177    
00178     mypwm.period_ms(10);
00179     mypwm.pulsewidth_ms(1);
00180     initServo(&servo, SERVO_PERIOD_MS, SERVO_PW_DEF_US); 
00181     printf("\nRadar Initialization done\n");      
00182     int angle =0;
00183     EWOK.reset();
00184     EWOK.stop();
00185     EWOK_INTR.rise(&EWOK_UP);
00186     EWOK_INTR.fall(&EWOK_DOWN);
00187      
00188     while(1){         
00189     
00190       //manual command of servo through terminal      
00191       printf("Use following keys\n");
00192       printf("\tEnter to start Radar\n");
00193       printf("\tt: to center the Radar\n");
00194       printf("\ty to set it to the right\n");
00195       printf("\tr to set it to the left\n");
00196       
00197       /*
00198       char c = device.getc();
00199       if(c == 'r') { //r-> left
00200                 servo.pulsewidth_us(2500);
00201       }
00202       if(c == 'y') { //y-> right
00203                 servo.pulsewidth_us(500);
00204       }
00205       
00206       if(c == 't') { //t-> center
00207                 servo.pulsewidth_us(1500);
00208       }
00209       
00210       if(c == '\r') { //enter = launching radar
00211       */
00212                
00213                printf("Enter pressed, starting radar\n");
00214                while(1)
00215                {
00216                 //printf("%i:%i\n", angle_servo, duration);
00217                 myUART.printf("%i:%i\n", angle_servo, duration);
00218                }
00219                
00220       /*
00221              if(c == 'r') { //r-> left
00222                 manualServo = 0;
00223                 manualServo = 1;
00224                 wait_us(2500);
00225                 manualServo = 0;
00226                 wait_us(1);
00227              }
00228              
00229              if(c == 't') {  // t-> right
00230                 manualServo = 0;
00231                 manualServo = 1;
00232                 wait_us(500);
00233                 manualServo = 0;
00234                 wait_us(1);
00235              }
00236              
00237              if(c == 'y') { 
00238                 manualServo = 0;
00239                 //wait_us(10);
00240                 manualServo = 1;
00241                 wait_us(1500);
00242                 manualServo = 0;
00243                 wait_us(1);
00244                 //wait_us(49500)  ;              
00245              }
00246         */        
00247         }
00248 }//main