Zürcher Eliteeinheit / Mbed 2 deprecated ROME2_P6

rome2_p6 imported

Dependencies:   mbed

Main.cpp@5:957580f33e52, 2018-05-18 (annotated)

Committer:
Appalco
Date:
Fri May 18 13:54:25 2018 +0000
Revision:
5:957580f33e52
Parent:
4:1c821a19876f 
fixed tolerance and wayponts

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Appalco 0:351a2fb21235 1 /*
Appalco 0:351a2fb21235 2 * Main.cpp
Appalco 0:351a2fb21235 3 * Copyright (c) 2018, ZHAW
Appalco 0:351a2fb21235 4 * All rights reserved.
Appalco 0:351a2fb21235 5 */
Appalco 0:351a2fb21235 6
Appalco 0:351a2fb21235 7 #include <cstdlib>
Appalco 0:351a2fb21235 8 #include <mbed.h>
Appalco 0:351a2fb21235 9 #include "EncoderCounter.h"
Appalco 0:351a2fb21235 10 #include "IRSensor.h"
Appalco 0:351a2fb21235 11 #include "IMU.h"
Appalco 0:351a2fb21235 12 #include "LIDAR.h"
Appalco 0:351a2fb21235 13 #include "Controller.h"
Appalco 0:351a2fb21235 14 #include "StateMachine.h"
Appalco 0:351a2fb21235 15 #include "SerialServer.h"
Appalco 5:957580f33e52 16 #define TOLERANCE 0.25f
Appalco 0:351a2fb21235 17
Appalco 0:351a2fb21235 18 int main() {
Appalco 0:351a2fb21235 19
Appalco 0:351a2fb21235 20 // create miscellaneous periphery objects
Appalco 1:4b956ba45704 21 float dist,angle,x_measured_r,y_measured_r, x_measured,y_measured,x,y,angle_pos;
Appalco 2:e3f6156788bf 22
Appalco 2:e3f6156788bf 23 // tolerance values for tube 1 and tube 2
Appalco 2:e3f6156788bf 24 const float min_x1 = 0.99f - TOLERANCE;
Appalco 2:e3f6156788bf 25 const float max_x1 = 0.99f + TOLERANCE;
Appalco 2:e3f6156788bf 26 const float min_y1 = 0.54f - TOLERANCE;
Appalco 2:e3f6156788bf 27 const float max_y1 = 0.54f + TOLERANCE;
Appalco 2:e3f6156788bf 28 const float min_x2 = 2.52f - TOLERANCE;
Appalco 2:e3f6156788bf 29 const float max_x2 = 2.52f + TOLERANCE;
Appalco 2:e3f6156788bf 30 const float min_y2 = 0.32f - TOLERANCE;
Appalco 2:e3f6156788bf 31 const float max_y2 = 0.32f + TOLERANCE;
Appalco 2:e3f6156788bf 32
Appalco 0:351a2fb21235 33 DigitalOut led(LED1);
Appalco 0:351a2fb21235 34 DigitalIn button(USER_BUTTON);
Appalco 0:351a2fb21235 35
Appalco 0:351a2fb21235 36 DigitalOut led0(PC_8);
Appalco 0:351a2fb21235 37 DigitalOut led1(PC_6);
Appalco 0:351a2fb21235 38 DigitalOut led2(PB_12);
Appalco 0:351a2fb21235 39 DigitalOut led3(PA_7);
Appalco 0:351a2fb21235 40 DigitalOut led4(PC_0);
Appalco 0:351a2fb21235 41 DigitalOut led5(PC_9);
Appalco 0:351a2fb21235 42
Appalco 0:351a2fb21235 43 // create motor control objects
Appalco 0:351a2fb21235 44
Appalco 0:351a2fb21235 45 DigitalOut enableMotorDriver(PB_2);
Appalco 0:351a2fb21235 46 DigitalIn motorDriverFault(PB_14);
Appalco 0:351a2fb21235 47 DigitalIn motorDriverWarning(PB_15);
Appalco 0:351a2fb21235 48
Appalco 0:351a2fb21235 49 PwmOut pwmLeft(PA_8);
Appalco 0:351a2fb21235 50 PwmOut pwmRight(PA_9);
Appalco 0:351a2fb21235 51
Appalco 0:351a2fb21235 52 EncoderCounter counterLeft(PB_6, PB_7);
Appalco 0:351a2fb21235 53 EncoderCounter counterRight(PA_6, PC_7);
Appalco 0:351a2fb21235 54
Appalco 0:351a2fb21235 55 // create distance sensor objects
Appalco 0:351a2fb21235 56
Appalco 0:351a2fb21235 57 DigitalOut enableIRSensors(PC_1);
Appalco 0:351a2fb21235 58 enableIRSensors = 1;
Appalco 0:351a2fb21235 59
Appalco 0:351a2fb21235 60 AnalogIn distance(PB_1);
Appalco 0:351a2fb21235 61 DigitalOut bit0(PH_1);
Appalco 0:351a2fb21235 62 DigitalOut bit1(PC_2);
Appalco 0:351a2fb21235 63 DigitalOut bit2(PC_3);
Appalco 0:351a2fb21235 64
Appalco 0:351a2fb21235 65 IRSensor irSensor0(distance, bit0, bit1, bit2, 0);
Appalco 0:351a2fb21235 66 IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
Appalco 0:351a2fb21235 67 IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
Appalco 0:351a2fb21235 68 IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
Appalco 0:351a2fb21235 69 IRSensor irSensor4(distance, bit0, bit1, bit2, 4);
Appalco 0:351a2fb21235 70 IRSensor irSensor5(distance, bit0, bit1, bit2, 5);
Appalco 0:351a2fb21235 71
Appalco 0:351a2fb21235 72 // create LIDAR object
Appalco 0:351a2fb21235 73
Appalco 0:351a2fb21235 74 PwmOut pwm(PA_10);
Appalco 0:351a2fb21235 75 pwm.period(0.00005f);
Appalco 0:351a2fb21235 76 pwm.write(0.5f); // 50% duty-cycle
Appalco 0:351a2fb21235 77
Appalco 0:351a2fb21235 78 RawSerial uart(PA_0, PA_1);
Appalco 0:351a2fb21235 79
Appalco 0:351a2fb21235 80 LIDAR lidar(uart);
Appalco 0:351a2fb21235 81
Appalco 0:351a2fb21235 82 // create robot controller objects
Appalco 0:351a2fb21235 83
Appalco 0:351a2fb21235 84 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
Appalco 0:351a2fb21235 85 StateMachine stateMachine(controller, enableMotorDriver, led0, led1, led2, led3, led4, led5, button, irSensor0, irSensor1, irSensor2, irSensor3, irSensor4, irSensor5);
Appalco 0:351a2fb21235 86
Appalco 0:351a2fb21235 87 // create serial server object
Appalco 0:351a2fb21235 88
Appalco 0:351a2fb21235 89 RawSerial serial(PB_10, PC_5);
Appalco 0:351a2fb21235 90 serial.baud(9600);
Appalco 0:351a2fb21235 91 serial.format(8, SerialBase::None, 1);
Appalco 0:351a2fb21235 92
Appalco 0:351a2fb21235 93 DigitalOut reset(PB_3);
Appalco 0:351a2fb21235 94 DigitalOut modes1(PB_4);
Appalco 0:351a2fb21235 95
Appalco 0:351a2fb21235 96 modes1 = 0;
Appalco 0:351a2fb21235 97
Appalco 0:351a2fb21235 98 reset = 1; wait(0.1f);
Appalco 0:351a2fb21235 99 reset = 0; wait(0.1f);
Appalco 0:351a2fb21235 100 reset = 1; wait(0.1f);
Appalco 0:351a2fb21235 101
Appalco 0:351a2fb21235 102 SerialServer serialServer(serial, lidar, controller);
Appalco 0:351a2fb21235 103
Appalco 3:09f77ebfd2e5 104 printf("created new waypoint,task");
Appalco 4:1c821a19876f 105
Appalco 0:351a2fb21235 106 while (true) {
Appalco 0:351a2fb21235 107
Appalco 4:1c821a19876f 108
Appalco 0:351a2fb21235 109 wait(0.2f); // wait for 200 ms
Appalco 0:351a2fb21235 110
Appalco 1:4b956ba45704 111 lidar.lookForBeacon();
Appalco 2:e3f6156788bf 112 dist = (float)lidar.getDistanceOfBeacon()/1000;
Appalco 4:1c821a19876f 113 angle = (float)lidar.getAngleOfBeacon()*3.14f/180;
Appalco 1:4b956ba45704 114 //relatives cord
Appalco 1:4b956ba45704 115 if(dist!=0)
Appalco 1:4b956ba45704 116 {
Appalco 5:957580f33e52 117 x_measured_r = dist*cos(angle);
Appalco 5:957580f33e52 118 y_measured_r = dist*cos(angle);
Appalco 1:4b956ba45704 119 //global cord
Appalco 1:4b956ba45704 120 angle_pos = controller.getAlpha();
Appalco 1:4b956ba45704 121 x= controller.getX();
Appalco 1:4b956ba45704 122 y= controller.getY();
Appalco 1:4b956ba45704 123 x_measured = cos(angle_pos)*x_measured_r-sin(angle_pos)*y_measured_r+x;
Appalco 1:4b956ba45704 124 y_measured = sin(angle_pos)*x_measured_r+cos(angle_pos)*y_measured_r+y;
Appalco 1:4b956ba45704 125 }
Appalco 2:e3f6156788bf 126 //check if measured tube is valid with reference
Appalco 2:e3f6156788bf 127 if(x_measured>=min_x1 && x_measured<= max_x1 && y_measured>=min_y1 && y_measured<=max_y1)
Appalco 2:e3f6156788bf 128 {
Appalco 2:e3f6156788bf 129 controller.correctPoseWithBeacon(0.99f,0.54f,x_measured,y_measured);
Appalco 2:e3f6156788bf 130 //turn led on
Appalco 2:e3f6156788bf 131 printf("found tube 1 at %f %f",x_measured,y_measured);
Appalco 4:1c821a19876f 132 led1 = !led1; // toggle led
Appalco 2:e3f6156788bf 133 }
Appalco 4:1c821a19876f 134 if(x_measured>=min_x2 && x_measured<= max_x2 && y_measured>=min_y2 && y_measured<=max_y2)
Appalco 2:e3f6156788bf 135 {
Appalco 2:e3f6156788bf 136 controller.correctPoseWithBeacon(2.52f,0.32f,x_measured,y_measured);
Appalco 2:e3f6156788bf 137 printf("found tube 2 at %f %f",x_measured,y_measured);
Appalco 2:e3f6156788bf 138 //turn led on
Appalco 4:1c821a19876f 139 led2 = !led2; // toggle led
Appalco 2:e3f6156788bf 140 }
Appalco 0:351a2fb21235 141 }
Appalco 2:e3f6156788bf 142
Appalco 0:351a2fb21235 143 }
Appalco 0:351a2fb21235 144