Alternative control of remote vehicles.
Diff: main.cpp
- Revision:
- 0:9e0195f256b1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Dec 13 15:04:55 2018 +0000
@@ -0,0 +1,356 @@
+#include "mbed.h"
+#include "XNucleo53L0A1.h"
+#include <stdio.h>
+
+Serial pc(USBTX,USBRX);
+
+// Setup the shutdown pins for the VL53L0Xs
+// When these pins are pulled LOW, the sensor goes into shutdown.
+
+DigitalOut center_shutx(p26);
+
+DigitalOut left_shdn(p18);
+
+DigitalOut center_shdn(p19);
+DigitalOut right_shdn(p20);
+
+
+// This VL53L0X board test application performs a range measurement in polling mode
+// Use 3.3(Vout) for Vin, p28 for SDA, p27 for SCL, P26 for shdn on mbed LPC1768
+
+//I2C sensor pins
+#define VL53L0_I2C_SDA p28
+#define VL53L0_I2C_SCL p27
+
+#define MY_DEBUG 0
+
+static XNucleo53L0A1 *board=NULL;
+
+// Orange = left
+// Blue = right
+// Yellow = forward
+// Green = backwards
+
+DigitalOut TurnLeft(p21);
+DigitalOut TurnRight(p22);
+//
+DigitalOut Forward(p23);
+DigitalOut Reverse(p24);
+
+void InitChannel(DevI2C *device_i2c, int WhichChannel)
+{
+ int status;
+
+ if(board != NULL)
+ {
+ // Call destructor
+ board->~XNucleo53L0A1();
+ board = NULL;
+ }
+
+ /* creates the 53L0A1 expansion board singleton obj */
+ board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);
+
+ left_shdn = 0;
+ right_shdn = 0;
+ center_shdn = 0;
+
+ switch(WhichChannel)
+ {
+ // left channel
+ case 0:
+
+ left_shdn = 1;
+
+ break;
+
+ // right channel
+ case 2:
+
+ right_shdn = 1;
+
+ break;
+
+ // center channel
+ case 1:
+ default:
+
+ center_shdn = 1;
+
+ break;
+ }
+
+ center_shutx = 0; //must reset sensor for an mbed reset to work
+ wait(0.001);
+ center_shutx = 1;
+ //wait(0.01);
+
+ /* init the 53L0A1 board with default values */
+ status = board->init_board();
+ while (status) {
+
+ if(MY_DEBUG) {
+ pc.printf("Failed to init board! \r\n");
+ }
+
+ status = board->init_board();
+ }
+}
+
+void TurnLeft()
+{
+ TurnLeft = 1;
+ wait(0.1);
+ TurnLeft = 0;
+}
+
+void TurnRight()
+{
+ TurnLeft = 0;
+ TurnRight = 1;
+ wait(0.1);
+ TurnRight = 0;
+}
+
+
+void Drive()
+{
+
+}
+
+void Reverse()
+{
+
+}
+
+
+
+
+
+
+uint32_t left_distance;
+uint32_t left_distance_prev;
+
+uint32_t center_distance;
+uint32_t center_distance_prev;
+
+uint32_t right_distance;
+uint32_t right_distance_prev;
+
+// Flashes all 4 leds with right channel selected after n minutes.
+// 12:31-12:32 start
+//
+
+
+int main()
+{
+ int status;
+ uint32_t distance;
+
+ TurnLeft = 0;
+ TurnRight = 0;
+ Forward = 0;
+ Reverse = 0;
+
+ left_distance = 2500;
+ left_distance_prev = 2500;
+
+ center_distance = 2500;
+ center_distance_prev = 2500;
+
+ right_distance = 2500;
+ right_distance_prev = 2500;
+
+ int state = 0;
+
+ DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
+
+ device_i2c->frequency(3000000);
+
+ // Init the center channel
+ InitChannel(device_i2c, 1);
+
+
+ //loop taking and printing distance
+ // bool didSwitchOver = false;
+ int i = 0;
+ bool CountUp = true;
+
+ uint32_t TheTest = 0;
+
+ while (1) {
+
+ status = board->sensor_centre->get_distance(&distance);
+
+ if (status == VL53L0X_ERROR_NONE) {
+ pc.printf("D=%ld mm\r\n", distance);
+
+
+ switch(state)
+ {
+ // Right
+ case 2:
+
+ TheTest = right_distance_prev;
+ right_distance_prev = right_distance;
+ right_distance = distance;
+
+ // Handle step functions (hopefully) gracefully.
+ if(TheTest <= 500 && distance > 500)
+ {
+ TurnRight = 0;
+
+ }
+ else
+ {
+ TheTest = (uint32_t)((right_distance + right_distance_prev)/2);
+
+ if( TheTest <= 500 )
+ {
+ TurnRight = 1;
+ }
+ else
+ {
+ TurnRight = 0;
+ }
+
+ }
+
+ break;
+
+ // Left
+ case 0:
+ TheTest = left_distance_prev;
+ left_distance_prev = left_distance;
+ left_distance = distance;
+
+ // Handle step functions (hopefully) gracefully.
+ if(TheTest <= 500 && distance > 500)
+ {
+ TurnLeft = 0;
+ }
+ else
+ {
+ TheTest = (uint32_t)((left_distance + left_distance_prev)/2);
+
+
+
+ if( TheTest <= 500 )
+ {
+ TurnLeft = 1;
+ }
+ else
+ {
+ TurnLeft = 0;
+ }
+
+ }
+
+ break;
+
+ // Center
+ case 1:
+ default:
+
+ TheTest = center_distance_prev;
+
+ center_distance_prev = center_distance;
+ center_distance = distance;
+
+ // Handle step functions (hopefully) gracefully.
+ if(TheTest <= 500 && distance > 500 )
+ {
+ Forward = 0;
+ Reverse = 0;
+ }
+ else
+ {
+
+ TheTest = (uint32_t)((center_distance + center_distance_prev)/2);
+
+ if( TheTest <= 200)
+ {
+ Forward = 1;
+ Reverse = 0;
+ }
+ else if( TheTest >= 200 && TheTest <= 500 )
+ {
+ Forward = 0;
+ Reverse = 1;
+ }
+ else
+ {
+ Forward = 0;
+ Reverse = 0;
+ }
+
+ }
+
+
+
+ break;
+ }
+
+
+
+
+
+ i++;
+ }
+ else
+ {
+ if(MY_DEBUG) {
+ pc.printf("COuld not get distance.\r\n");
+ }
+ distance = 2500; // 2500 mm
+ }
+
+ // Take 5 measurements on the channel
+ if( i > 2) {
+
+ if(CountUp)
+ {
+
+ if( state == 2 )
+ {
+ state--;
+ CountUp = false;
+ }
+ else
+ {
+ state++;
+ CountUp = true;
+ }
+
+ }
+ else
+ {
+ if( state == 0 )
+ {
+ state++;
+ CountUp = true;
+ }
+ else
+ {
+ state--;
+ CountUp = false;
+ }
+ }
+
+ if(MY_DEBUG) {
+ pc.printf("Doing switch over\r\n");
+ }
+ InitChannel(device_i2c, state);
+
+ if(MY_DEBUG) {
+ pc.printf("Finishing switch over\r\n");
+ }
+
+ i = 0;
+ }
+ }
+}
+
+
+
+