Ngoc Trong Nghia NGUYEN / Mbed 2 deprecated 6_laser_avec_servo

Dependencies:   mbed vl53l0x_api Servolib

Files at this revision

API Documentation at this revision

Comitter:
nguyentony
Date:
Tue Apr 02 13:23:15 2019 +0000
Commit message:
contournement;

Changed in this revision

Servolib.lib Show annotated file Show diff for this revision Revisions of this file
capteur_laser.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
vl53l0x_api.lib Show annotated file Show diff for this revision Revisions of this file
--- /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