Main.cpp

00001 /*
00002  * Main.cpp
00003  * Copyright (c) 2018, ZHAW
00004  * All rights reserved.
00005  */
00006 
00007 #include <cstdlib>
00008 #include <mbed.h>
00009 #include "EncoderCounter.h"
00010 #include "IRSensor.h"
00011 #include "IMU.h"
00012 #include "LIDAR.h"
00013 #include "Controller.h"
00014 #include "StateMachine.h"
00015 #include "SerialServer.h"
00016 #define TOLERANCE 0.25f
00017 
00018 int main() {
00019     
00020     // create miscellaneous periphery objects
00021     float dist,angle,x_measured_r,y_measured_r, x_measured,y_measured,x,y,angle_pos;
00022     
00023     // tolerance values for tube 1 and tube 2
00024     const float min_x1 = 0.99f - TOLERANCE;
00025     const float max_x1 = 0.99f + TOLERANCE;
00026     const float min_y1 = 0.54f - TOLERANCE;
00027     const float max_y1 = 0.54f + TOLERANCE;
00028     const float min_x2 = 2.52f - TOLERANCE;
00029     const float max_x2 = 2.52f + TOLERANCE;
00030     const float min_y2 = 0.32f - TOLERANCE;
00031     const float max_y2 = 0.32f + TOLERANCE;
00032     
00033     DigitalOut led(LED1);
00034     DigitalIn button(USER_BUTTON);
00035     
00036     DigitalOut led0(PC_8);
00037     DigitalOut led1(PC_6);
00038     DigitalOut led2(PB_12);
00039     DigitalOut led3(PA_7);
00040     DigitalOut led4(PC_0);
00041     DigitalOut led5(PC_9);
00042     
00043     // create motor control objects
00044     
00045     DigitalOut enableMotorDriver(PB_2);
00046     DigitalIn motorDriverFault(PB_14);
00047     DigitalIn motorDriverWarning(PB_15);
00048     
00049     PwmOut pwmLeft(PA_8);
00050     PwmOut pwmRight(PA_9);
00051     
00052     EncoderCounter counterLeft(PB_6, PB_7);
00053     EncoderCounter counterRight(PA_6, PC_7);
00054     
00055     // create distance sensor objects
00056     
00057     DigitalOut enableIRSensors(PC_1);
00058     enableIRSensors = 1;
00059     
00060     AnalogIn distance(PB_1);
00061     DigitalOut bit0(PH_1);
00062     DigitalOut bit1(PC_2);
00063     DigitalOut bit2(PC_3);
00064     
00065     IRSensor irSensor0(distance, bit0, bit1, bit2, 0);
00066     IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
00067     IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
00068     IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
00069     IRSensor irSensor4(distance, bit0, bit1, bit2, 4);
00070     IRSensor irSensor5(distance, bit0, bit1, bit2, 5);
00071     
00072     // create LIDAR object
00073     
00074     PwmOut pwm(PA_10);
00075     pwm.period(0.00005f);
00076     pwm.write(0.5f);      // 50% duty-cycle
00077     
00078     RawSerial uart(PA_0, PA_1);
00079     
00080     LIDAR lidar(uart);
00081     
00082     // create robot controller objects
00083     
00084     Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
00085     StateMachine stateMachine(controller, enableMotorDriver, led0, led1, led2, led3, led4, led5, button, irSensor0, irSensor1, irSensor2, irSensor3, irSensor4, irSensor5);
00086     
00087     // create serial server object
00088     
00089     RawSerial serial(PB_10, PC_5);
00090     serial.baud(9600);
00091     serial.format(8, SerialBase::None, 1);
00092     
00093     DigitalOut reset(PB_3);
00094     DigitalOut modes1(PB_4);
00095     
00096     modes1 = 0;
00097     
00098     reset = 1; wait(0.1f);
00099     reset = 0; wait(0.1f);
00100     reset = 1; wait(0.1f);
00101     
00102     SerialServer serialServer(serial, lidar, controller);
00103     
00104     printf("created new waypoint,task");
00105  
00106     while (true) {
00107         
00108     
00109         wait(0.2f); // wait for 200 ms
00110         
00111         lidar.lookForBeacon();
00112         dist =  (float)lidar.getDistanceOfBeacon()/1000;
00113         angle = (float)lidar.getAngleOfBeacon()*3.14f/180;
00114         //relatives cord
00115         if(dist!=0)
00116         {
00117             x_measured_r = dist*cos(angle);
00118             y_measured_r = dist*cos(angle);
00119             //global cord
00120             angle_pos = controller.getAlpha();
00121             x= controller.getX(); 
00122             y= controller.getY();
00123             x_measured = cos(angle_pos)*x_measured_r-sin(angle_pos)*y_measured_r+x;
00124             y_measured = sin(angle_pos)*x_measured_r+cos(angle_pos)*y_measured_r+y;
00125         }
00126        //check if measured tube is valid with reference
00127        if(x_measured>=min_x1 && x_measured<= max_x1 && y_measured>=min_y1 && y_measured<=max_y1)
00128         {
00129             controller.correctPoseWithBeacon(0.99f,0.54f,x_measured,y_measured);
00130             //turn led on
00131             printf("found tube 1 at %f %f",x_measured,y_measured);
00132             led1 = !led1; // toggle led
00133         }
00134        if(x_measured>=min_x2 && x_measured<= max_x2 && y_measured>=min_y2 && y_measured<=max_y2)
00135         {
00136             controller.correctPoseWithBeacon(2.52f,0.32f,x_measured,y_measured);
00137             printf("found tube 2 at %f %f",x_measured,y_measured);
00138             //turn led on
00139             led2 = !led2; // toggle led
00140         }
00141     }
00142     
00143 }
00144