main.cpp

00001 #include <stdint.h>
00002 #include <stdio.h>
00003 #include "mbed.h"
00004 #include "XNucleo53L1A1.h"
00005 #include "VL53L1X_I2C.h"
00006 
00007 uint16_t distance_centre = 0;
00008 uint16_t distance_left = 0;
00009 int16_t distance_right = 0;
00010 uint8_t state =0;
00011 uint8_t command = 0;
00012 int wait_fun();
00013 
00014 void speed(void)
00015 {
00016     if (command == 1 ) {
00017         printf("go forward, speed = %d\r\n",distance_centre - 80);
00018     } else if (command == 2) {
00019         printf("go back, speed = %d\r\n",50 - distance_centre);
00020     } else if (command == 3) {
00021         printf("Waiting, speed = 0\r\n");
00022     } else if (command == 7) {
00023         printf("turn right\r\n");
00024     } else if (command == 8) {
00025         printf("turn left\r\n");
00026     } else if (command ==5) {
00027         printf("left forward, speed = %d\r\n",distance_centre - 80);
00028     } else if (command ==55) {
00029         printf("left backward, speed = %d\r\n",50 - distance_centre);
00030     } else if (command == 6) {
00031         printf ("right forward, speed = %d\r\n",distance_centre - 80);
00032     } else if (command == 66) {
00033         printf ("right backward, speed = %d\r\n",50 - distance_centre);
00034     } else if (command == 4) {
00035         printf("Stop\r\n");
00036     }
00037 }
00038 
00039 void main()
00040 {
00041     speed();
00042 }