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?
User | Revision | Line number | New 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 |