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 vl53l0x_api Servolib
Revision 0:1806894cdd53, committed 2019-04-02
- Comitter:
- nguyentony
- Date:
- Tue Apr 02 13:23:15 2019 +0000
- Commit message:
- contournement;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servolib.lib Tue Apr 02 13:23:15 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/TABLE4/code/Servolib/#2f33e8615692
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/capteur_laser.h Tue Apr 02 13:23:15 2019 +0000 @@ -0,0 +1,100 @@ +#include "vl53l0x_api.h" +#include "vl53l0x_platform.h" +#include "vl53l0x_i2c_platform.h" + +#define USE_I2C_2V8 + +VL53L0X_Error WaitMeasurementDataReady(VL53L0X_DEV Dev) { + VL53L0X_Error Status = VL53L0X_ERROR_NONE; + uint8_t NewDatReady=0; + uint32_t LoopNb; + + if (Status == VL53L0X_ERROR_NONE) { + LoopNb = 0; + do { + Status = VL53L0X_GetMeasurementDataReady(Dev, &NewDatReady); + if ((NewDatReady == 0x01) || Status != VL53L0X_ERROR_NONE) { + break; + } + LoopNb = LoopNb + 1; + VL53L0X_PollingDelay(Dev); + } while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP); + + if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) { + Status = VL53L0X_ERROR_TIME_OUT; + } + } + + return Status; +} + +VL53L0X_Error WaitStopCompleted(VL53L0X_DEV Dev) { + VL53L0X_Error Status = VL53L0X_ERROR_NONE; + uint32_t StopCompleted=0; + uint32_t LoopNb; + + if (Status == VL53L0X_ERROR_NONE) { + LoopNb = 0; + do { + Status = VL53L0X_GetStopCompletedStatus(Dev, &StopCompleted); + if ((StopCompleted == 0x00) || Status != VL53L0X_ERROR_NONE) { + break; + } + LoopNb = LoopNb + 1; + VL53L0X_PollingDelay(Dev); + } while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP); + + if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) { + Status = VL53L0X_ERROR_TIME_OUT; + } + + } + + return Status; +} + +VL53L0X_Dev_t MyDevice; +VL53L0X_Dev_t *pMyDevice = &MyDevice; +VL53L0X_RangingMeasurementData_t RangingMeasurementData; +VL53L0X_RangingMeasurementData_t *pRangingMeasurementData = &RangingMeasurementData; + +void laser_init() +{ + // Initialize Comms + pMyDevice->I2cDevAddr = 0x52; + pMyDevice->comms_type = 1; + pMyDevice->comms_speed_khz = 400; + + + //VL53L0X_ERROR_CONTROL_INTERFACE; + VL53L0X_RdWord(&MyDevice, VL53L0X_REG_OSC_CALIBRATE_VAL,0); + VL53L0X_DataInit(&MyDevice); // Data initialization + //Status = VL53L0X_ERROR_NONE; + uint32_t refSpadCount; + uint8_t isApertureSpads; + uint8_t VhvSettings; + uint8_t PhaseCal; + + VL53L0X_StaticInit(pMyDevice); // Device Initialization + VL53L0X_PerformRefSpadManagement(pMyDevice, &refSpadCount, &isApertureSpads); // Device Initialization + VL53L0X_PerformRefCalibration(pMyDevice, &VhvSettings, &PhaseCal); // Device Initialization + VL53L0X_SetDeviceMode(pMyDevice, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in single ranging mode + VL53L0X_StartMeasurement(pMyDevice); +} + +int laser_mesure(int nb_mesure) +{ + int measure=0; + int sum=0; + for(int i=0; i<nb_mesure; i++){ + WaitMeasurementDataReady(pMyDevice); + VL53L0X_GetRangingMeasurementData(pMyDevice, pRangingMeasurementData); + measure=pRangingMeasurementData->RangeMilliMeter; + //printf("In loop measurement %d\n", mea); + sum=sum+measure; + // Clear the interrupt + VL53L0X_ClearInterruptMask(pMyDevice, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY); + VL53L0X_PollingDelay(pMyDevice); + } + return sum/nb_mesure; +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Apr 02 13:23:15 2019 +0000 @@ -0,0 +1,67 @@ +#include "mbed.h" +#include "capteur_laser.h" +#include "Servo.h" + +Servo servo_laser(A3); +int dist_mesure[9]; + + +#define GAIN_CONTOURNER 2 +#define ALPHA0 20 +/* + * INPUT: d: le pointeur vers un tableau de 9 valeurs de distance + dirR: la direction du Robot, recuperee par la boussole + * OUTPUT: nouvelle_consigne: l'angle absolute pour contourner dse obstacles + */ + +float contournement(int* d, float dirR) +{ + float k; + float angle_tourner; + float nouvelle_consigne; + float somme_d = 0; + float somme_d_k = 0; + for(int i=0; i<9; i++) + { + k = i-4; + somme_d += (float)d[i]; + somme_d_k += k*(float)d[i]; + } + angle_tourner = (somme_d_k/somme_d)*ALPHA0*GAIN_CONTOURNER; + if(angle_tourner>90) angle_tourner=90; + if(angle_tourner<-90) angle_tourner=-90; + nouvelle_consigne = angle_tourner + dirR; + return nouvelle_consigne; +} + +int main() +{ + float nouvelle_angle; + laser_init(); + servo_laser.set_position(0); + wait_ms(300); + int i=0; + + for(int d=0;d<=180;d += 20) + { + servo_laser.set_position(d); + wait_ms(100); + dist_mesure[i] = laser_mesure(3); + if(dist_mesure[i] > 1200) dist_mesure[i] = 1200; + i++; + //printf("Average is %d\n\r", ave); + } + printf("\n\n\r*******\n\r"); + for(i=0; i<9; i++) + { + printf("%d\n\r", dist_mesure[i]); + } + printf("\n\n\r*******\n\r"); + + nouvelle_angle = contournement(dist_mesure, 0); + printf("%.2f\n\r", nouvelle_angle); + + while (1) + { + } +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Apr 02 13:23:15 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/vl53l0x_api.lib Tue Apr 02 13:23:15 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/nguyentony/code/vl53l0x_api/#74763ee81c17