main.cpp

00001 #include "LidarLitev2.h"
00002 #include "DRV8825.h"
00003 #include "MODSERIAL.h"
00004 #include "math.h"
00005 
00006 #define PI 3.14159265359
00007 
00008 Serial pc(USBTX, USBRX);
00009 
00010 DRV8825 stpr_mtr(p21, p22, p23, p24, p25, p26);
00011 DigitalIn pinMode(p19); 
00012 
00013 LidarLitev2 Lidar(p28, p27);
00014 
00015 
00016 int main()
00017 {   
00018     stpr_mtr.direction = 1;
00019     pc.baud(9600);
00020     Lidar.configure();
00021     //Lidar.beginContinuous();
00022     float degree = 0, x, y;
00023     int distance;
00024     char e;
00025     while(1){
00026          if(pc.readable()) {
00027              e = pc.getc();
00028         }
00029         while (e == 'e'){
00030          //if(!pinMode) {  //Check if the lidar is pulling low
00031             distance = Lidar.distance();
00032             x = distance*cos(degree*(PI/180));
00033             y =  distance*sin(degree*(PI/180));
00034             stpr_mtr.settings(1, 0, 400);
00035             degree += .45;
00036             if(degree >= 360) degree = 0;
00037             //pc.printf("%4d,%4.0f\n", distance, degree);
00038             pc.printf("%4.0f,%4.0f\n", x, y);
00039           //}
00040           if(pc.readable()) {
00041              e = pc.getc();
00042             }
00043           if (e == 's') break;
00044         }
00045     }
00046 }