TeamSurface / Mbed 2 deprecated ROME2_P2

P2

Dependencies:   Library mbed

main.cpp@0:b15fac82ee4e, 2018-03-09 (annotated)

Committer:
kueenste
Date:
Fri Mar 09 15:30:02 2018 +0000
Revision:
0:b15fac82ee4e
Child:
1:ca37d1831bd1
drgtet

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kueenste 0:b15fac82ee4e 1 #include "mbed.h"
kueenste 0:b15fac82ee4e 2 #include "IRSensor.h"
kueenste 0:b15fac82ee4e 3 #include "Controller.h"
kueenste 0:b15fac82ee4e 4
kueenste 0:b15fac82ee4e 5 // User Button
kueenste 0:b15fac82ee4e 6 DigitalIn button(USER_BUTTON);
kueenste 0:b15fac82ee4e 7
kueenste 0:b15fac82ee4e 8 // LED
kueenste 0:b15fac82ee4e 9
kueenste 0:b15fac82ee4e 10 DigitalOut led0(PC_8);
kueenste 0:b15fac82ee4e 11 DigitalOut led1(PC_6);
kueenste 0:b15fac82ee4e 12 DigitalOut led2(PB_12);
kueenste 0:b15fac82ee4e 13 DigitalOut led3(PA_7);
kueenste 0:b15fac82ee4e 14 DigitalOut led4(PC_0);
kueenste 0:b15fac82ee4e 15 DigitalOut led5(PC_9);
kueenste 0:b15fac82ee4e 16
kueenste 0:b15fac82ee4e 17 // Distanzmesser
kueenste 0:b15fac82ee4e 18 AnalogIn distance(PB_1); // Kreieren der Ein- und Ausgangsobjekte
kueenste 0:b15fac82ee4e 19 DigitalOut enable(PC_1);
kueenste 0:b15fac82ee4e 20 DigitalOut bit0(PH_1);
kueenste 0:b15fac82ee4e 21 DigitalOut bit1(PC_2);
kueenste 0:b15fac82ee4e 22 DigitalOut bit2(PC_3);
kueenste 0:b15fac82ee4e 23
kueenste 0:b15fac82ee4e 24 // Motoren
kueenste 0:b15fac82ee4e 25 DigitalOut enableMotorDriver(PB_2);
kueenste 0:b15fac82ee4e 26 //DigitalIn motorDriverFault(PB_14);
kueenste 0:b15fac82ee4e 27 //DigitalIn motorDriverWarning(PB_15);
kueenste 0:b15fac82ee4e 28 PwmOut pwmLeft(PA_8);
kueenste 0:b15fac82ee4e 29 PwmOut pwmRight(PA_9);
kueenste 0:b15fac82ee4e 30
kueenste 0:b15fac82ee4e 31 // Motoren Drehzahlgeber
kueenste 0:b15fac82ee4e 32 EncoderCounter counterLeft(PB_6, PB_7);
kueenste 0:b15fac82ee4e 33 EncoderCounter counterRight(PA_6, PC_7);
kueenste 0:b15fac82ee4e 34
kueenste 0:b15fac82ee4e 35 enum { robOff, robMove, robSlowDown, robLeft, robRight };
kueenste 0:b15fac82ee4e 36
kueenste 0:b15fac82ee4e 37 int state;
kueenste 0:b15fac82ee4e 38
kueenste 0:b15fac82ee4e 39
kueenste 0:b15fac82ee4e 40 // Objects
kueenste 0:b15fac82ee4e 41 IRSensor irSensor0(distance, bit0, bit1, bit2, 0);
kueenste 0:b15fac82ee4e 42 IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
kueenste 0:b15fac82ee4e 43 IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
kueenste 0:b15fac82ee4e 44 IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
kueenste 0:b15fac82ee4e 45 IRSensor irSensor4(distance, bit0, bit1, bit2, 4);
kueenste 0:b15fac82ee4e 46 IRSensor irSensor5(distance, bit0, bit1, bit2, 5);
kueenste 0:b15fac82ee4e 47
kueenste 0:b15fac82ee4e 48 int main() {
kueenste 0:b15fac82ee4e 49
kueenste 0:b15fac82ee4e 50 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
kueenste 0:b15fac82ee4e 51
kueenste 0:b15fac82ee4e 52 printf("\n============== ROME P2 ===================\n");
kueenste 0:b15fac82ee4e 53
kueenste 0:b15fac82ee4e 54 enable = 1; // schaltet die Sensoren ein
kueenste 0:b15fac82ee4e 55
kueenste 0:b15fac82ee4e 56 state = robOff;
kueenste 0:b15fac82ee4e 57
kueenste 0:b15fac82ee4e 58 // MOTOR TEST
kueenste 0:b15fac82ee4e 59 /*
kueenste 0:b15fac82ee4e 60 enableMotorDriver = 1;
kueenste 0:b15fac82ee4e 61 wait(3);
kueenste 0:b15fac82ee4e 62 controller.setTranslationalVelocity(10);
kueenste 0:b15fac82ee4e 63
kueenste 0:b15fac82ee4e 64 wait(5);
kueenste 0:b15fac82ee4e 65 controller.setTranslationalVelocity(5);
kueenste 0:b15fac82ee4e 66 wait(5);
kueenste 0:b15fac82ee4e 67 controller.setTranslationalVelocity(10);
kueenste 0:b15fac82ee4e 68 */
kueenste 0:b15fac82ee4e 69
kueenste 0:b15fac82ee4e 70 while(1) {
kueenste 0:b15fac82ee4e 71
kueenste 0:b15fac82ee4e 72 // Initial reads
kueenste 0:b15fac82ee4e 73
kueenste 0:b15fac82ee4e 74 float distanceStraight = irSensor0.read(); // gibt die Distanz in [m] zurueck
kueenste 0:b15fac82ee4e 75 float distanceRight = irSensor1.read();
kueenste 0:b15fac82ee4e 76 float distanceBackRight = irSensor2.read();
kueenste 0:b15fac82ee4e 77 float distanceBack = irSensor3.read();
kueenste 0:b15fac82ee4e 78 float distanceBackLeft = irSensor4.read();
kueenste 0:b15fac82ee4e 79 float distanceLeft = irSensor5.read();
kueenste 0:b15fac82ee4e 80
kueenste 0:b15fac82ee4e 81 // FSM
kueenste 0:b15fac82ee4e 82
kueenste 0:b15fac82ee4e 83 switch(state) {
kueenste 0:b15fac82ee4e 84
kueenste 0:b15fac82ee4e 85 // Robot off
kueenste 0:b15fac82ee4e 86 case(robOff):
kueenste 0:b15fac82ee4e 87
kueenste 0:b15fac82ee4e 88 controller.setTranslationalVelocity(0.0f);
kueenste 0:b15fac82ee4e 89 controller.setRotationalVelocity(0.0f);
kueenste 0:b15fac82ee4e 90
kueenste 0:b15fac82ee4e 91 enableMotorDriver = 0; // Schaltet den Leistungstreiber
kueenste 0:b15fac82ee4e 92
kueenste 0:b15fac82ee4e 93 // button pressed
kueenste 0:b15fac82ee4e 94 if (button.read()){
kueenste 0:b15fac82ee4e 95 enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
kueenste 0:b15fac82ee4e 96
kueenste 0:b15fac82ee4e 97 state = robMove;
kueenste 0:b15fac82ee4e 98 }
kueenste 0:b15fac82ee4e 99
kueenste 0:b15fac82ee4e 100 break;
kueenste 0:b15fac82ee4e 101 // Move Forward
kueenste 0:b15fac82ee4e 102 case(robMove):
kueenste 0:b15fac82ee4e 103
kueenste 0:b15fac82ee4e 104 controller.setTranslationalVelocity(20.0f);
kueenste 0:b15fac82ee4e 105 controller.setRotationalVelocity(0.0f);
kueenste 0:b15fac82ee4e 106
kueenste 0:b15fac82ee4e 107
kueenste 0:b15fac82ee4e 108 // button pressed
kueenste 0:b15fac82ee4e 109 if (button.read()){
kueenste 0:b15fac82ee4e 110 state = robOff;
kueenste 0:b15fac82ee4e 111 } else if(distanceStraight <= 0.4f) {
kueenste 0:b15fac82ee4e 112 if(distanceLeft > 0.3f) {
kueenste 0:b15fac82ee4e 113 state = robLeft;
kueenste 0:b15fac82ee4e 114 } else if(distanceRight > 0.3f) {
kueenste 0:b15fac82ee4e 115 state = robRight;
kueenste 0:b15fac82ee4e 116 } else {
kueenste 0:b15fac82ee4e 117 state = robSlowDown;
kueenste 0:b15fac82ee4e 118 }
kueenste 0:b15fac82ee4e 119 }
kueenste 0:b15fac82ee4e 120 break;
kueenste 0:b15fac82ee4e 121 // Slowing Down
kueenste 0:b15fac82ee4e 122 case(robSlowDown):
kueenste 0:b15fac82ee4e 123
kueenste 0:b15fac82ee4e 124 controller.setTranslationalVelocity(0.0f);
kueenste 0:b15fac82ee4e 125 controller.setRotationalVelocity(0.0f);
kueenste 0:b15fac82ee4e 126
kueenste 0:b15fac82ee4e 127 if(abs(controller.getActualTranslationalVelocity()) < 0.01f && abs(controller.getActualRotationalVelocity()) < 0.01f) {
kueenste 0:b15fac82ee4e 128 state = robOff;
kueenste 0:b15fac82ee4e 129 }
kueenste 0:b15fac82ee4e 130
kueenste 0:b15fac82ee4e 131
kueenste 0:b15fac82ee4e 132 break;
kueenste 0:b15fac82ee4e 133 // Turn Left
kueenste 0:b15fac82ee4e 134 case(robLeft):
kueenste 0:b15fac82ee4e 135
kueenste 0:b15fac82ee4e 136 controller.setTranslationalVelocity(0.0f);
kueenste 0:b15fac82ee4e 137 controller.setRotationalVelocity(10.0f);
kueenste 0:b15fac82ee4e 138
kueenste 0:b15fac82ee4e 139
kueenste 0:b15fac82ee4e 140 // button pressed
kueenste 0:b15fac82ee4e 141 if (button.read()){
kueenste 0:b15fac82ee4e 142 state = robSlowDown;
kueenste 0:b15fac82ee4e 143 } else if(distanceStraight > 0.6f) {
kueenste 0:b15fac82ee4e 144 state = robMove;
kueenste 0:b15fac82ee4e 145 }
kueenste 0:b15fac82ee4e 146 break;
kueenste 0:b15fac82ee4e 147 // Turn Right
kueenste 0:b15fac82ee4e 148 case(robRight):
kueenste 0:b15fac82ee4e 149
kueenste 0:b15fac82ee4e 150 controller.setTranslationalVelocity(0.0f);
kueenste 0:b15fac82ee4e 151 controller.setRotationalVelocity(-10.0f);
kueenste 0:b15fac82ee4e 152
kueenste 0:b15fac82ee4e 153 // button pressed
kueenste 0:b15fac82ee4e 154 if (button.read()){
kueenste 0:b15fac82ee4e 155 state = robSlowDown;
kueenste 0:b15fac82ee4e 156 } else if(distanceStraight > 0.6f) {
kueenste 0:b15fac82ee4e 157 state = robMove;
kueenste 0:b15fac82ee4e 158 }
kueenste 0:b15fac82ee4e 159 break;
kueenste 0:b15fac82ee4e 160 default:
kueenste 0:b15fac82ee4e 161 // fuck off
kueenste 0:b15fac82ee4e 162
kueenste 0:b15fac82ee4e 163 break;
kueenste 0:b15fac82ee4e 164
kueenste 0:b15fac82ee4e 165 }
kueenste 0:b15fac82ee4e 166
kueenste 0:b15fac82ee4e 167
kueenste 0:b15fac82ee4e 168 }
kueenste 0:b15fac82ee4e 169 }