Helvijs Kiselis / Mbed 2 deprecated MicromousePES2

main.cpp: Sensoren einlesen und Motoren ansteuern

Dependencies:   mbed

main.cpp@7:8d60a2ff4c2b, 2018-04-16 (annotated)

Committer:
maddajan
Date:
Mon Apr 16 12:49:50 2018 +0000
Revision:
7:8d60a2ff4c2b
Parent:
6:1f084e6f7e80 
rfesfs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Helvis 0:9a3e7847a4be 1 #include <mbed.h>
Helvis 0:9a3e7847a4be 2
Helvis 0:9a3e7847a4be 3 #include "EncoderCounter.h"
Helvis 0:9a3e7847a4be 4 #include "Controller.h"
Helvis 0:9a3e7847a4be 5 #include "IRSensor.h"
Helvis 1:1adf5dfcc7bb 6 #include "Motion.h"
Helvis 0:9a3e7847a4be 7
Helvis 1:1adf5dfcc7bb 8 //User Button
Helvis 5:47262622a9bf 9 InterruptIn button(USER_BUTTON);
maddajan 7:8d60a2ff4c2b 10 //ciaone
Helvis 1:1adf5dfcc7bb 11
Helvis 1:1adf5dfcc7bb 12 //Sensors:
Helvis 0:9a3e7847a4be 13
Helvis 5:47262622a9bf 14 AnalogIn lineSensor(PA_4);
Helvis 5:47262622a9bf 15 AnalogIn distance2(PC_3);
Helvis 5:47262622a9bf 16 AnalogIn distance4(PB_1);
Helvis 5:47262622a9bf 17 AnalogIn distance1(PC_2);
Helvis 5:47262622a9bf 18 AnalogIn distance3(PC_5);
Helvis 0:9a3e7847a4be 19
Helvis 5:47262622a9bf 20 IRSensor irSensorL (distance2);
Helvis 5:47262622a9bf 21 IRSensor irSensorC (distance4);
Helvis 5:47262622a9bf 22 IRSensor irSensorR (distance1);
Helvis 5:47262622a9bf 23 IRSensor irSensorB (distance3);
Helvis 0:9a3e7847a4be 24
Helvis 1:1adf5dfcc7bb 25 //Motors:
Helvis 0:9a3e7847a4be 26 DigitalOut myled(LED1);
Helvis 0:9a3e7847a4be 27
Helvis 0:9a3e7847a4be 28 DigitalOut enableMotorDriver(PB_2);
Helvis 0:9a3e7847a4be 29
Helvis 0:9a3e7847a4be 30 DigitalIn motorDriverFault(PB_14);
Helvis 0:9a3e7847a4be 31 DigitalIn motorDriverWarning(PB_15);
Helvis 0:9a3e7847a4be 32
Helvis 5:47262622a9bf 33 PwmOut pwmRight(PA_8);
Helvis 5:47262622a9bf 34 PwmOut pwmLeft(PA_10);
Helvis 0:9a3e7847a4be 35
Helvis 5:47262622a9bf 36 EncoderCounter counterRight(PB_6, PB_7);
Helvis 5:47262622a9bf 37 EncoderCounter counterLeft(PA_0, PA_1);
Helvis 5:47262622a9bf 38
Helvis 0:9a3e7847a4be 39
Helvis 0:9a3e7847a4be 40 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
Helvis 1:1adf5dfcc7bb 41
Helvis 5:47262622a9bf 42 Motion motion(controller, counterLeft, counterRight, irSensorL, irSensorC, irSensorR, enableMotorDriver);
Helvis 4:e74c06e43485 43
Helvis 5:47262622a9bf 44 //------------------------------------------------------------------------------
Helvis 5:47262622a9bf 45
Helvis 5:47262622a9bf 46 volatile int start = 0;
Helvis 4:e74c06e43485 47 short countsLeft = 0;
Helvis 4:e74c06e43485 48 short countsRight = 0;
Helvis 5:47262622a9bf 49 short countsLeftOld = 0;
Helvis 5:47262622a9bf 50 short countsRightOld = 0;
Helvis 5:47262622a9bf 51 //den jeweiligen Test auf 1 setzen
Helvis 5:47262622a9bf 52 int testSensor = 0;
Helvis 5:47262622a9bf 53 int testMotion = 0;
Helvis 5:47262622a9bf 54 int testRotation = 0;
Helvis 5:47262622a9bf 55 int testCounts = 0;
Helvis 5:47262622a9bf 56 int testClass = 0;
Helvis 5:47262622a9bf 57
Helvis 5:47262622a9bf 58 //------------------------------------------------------------------------------
Helvis 5:47262622a9bf 59
Helvis 5:47262622a9bf 60 //User button toggle
Helvis 5:47262622a9bf 61 void press() {
Helvis 5:47262622a9bf 62 start = !start;
Helvis 5:47262622a9bf 63 }
Helvis 5:47262622a9bf 64
Helvis 4:e74c06e43485 65
Helvis 4:e74c06e43485 66 //------------------------------------------------------------------------------
Helvis 0:9a3e7847a4be 67
Helvis 0:9a3e7847a4be 68 int main() {
Helvis 0:9a3e7847a4be 69
Helvis 4:e74c06e43485 70 /**
Helvis 4:e74c06e43485 71 Micromouse Test
Helvis 1:1adf5dfcc7bb 72 */
Helvis 1:1adf5dfcc7bb 73
Helvis 5:47262622a9bf 74
Helvis 5:47262622a9bf 75 while(1) {
Helvis 5:47262622a9bf 76
Helvis 5:47262622a9bf 77 button.fall(&press); //User button einlesen
Helvis 4:e74c06e43485 78
Helvis 5:47262622a9bf 79 while(0) {
Helvis 5:47262622a9bf 80 myled = 1;
Helvis 5:47262622a9bf 81 printf("Hai\n");
Helvis 5:47262622a9bf 82 if (start == 0) break;
Helvis 5:47262622a9bf 83 }
Helvis 5:47262622a9bf 84
Helvis 0:9a3e7847a4be 85
Helvis 0:9a3e7847a4be 86
Helvis 1:1adf5dfcc7bb 87 /*
Helvis 1:1adf5dfcc7bb 88 Sensoren Test
Helvis 1:1adf5dfcc7bb 89 */
Helvis 4:e74c06e43485 90 while(1) {
Helvis 4:e74c06e43485 91
Helvis 0:9a3e7847a4be 92
Helvis 5:47262622a9bf 93 float distanceL = irSensorL.readL();
Helvis 5:47262622a9bf 94 float distanceC = irSensorC.readC();
Helvis 5:47262622a9bf 95 float distanceR = irSensorR.readR();
Helvis 5:47262622a9bf 96 float distanceB = irSensorB.readC();
Helvis 5:47262622a9bf 97 printf("Links: %f Mitte: %f Rechts: %f Hinten: %f Line: %f\n", distanceL, distanceC, distanceR, distanceB, lineSensor.read());
Helvis 0:9a3e7847a4be 98
Helvis 5:47262622a9bf 99 //wait(0.5f);
Helvis 0:9a3e7847a4be 100 }
Helvis 0:9a3e7847a4be 101
Helvis 1:1adf5dfcc7bb 102 /*
Helvis 1:1adf5dfcc7bb 103 Motoren Test: Gerade Bewegung
Helvis 1:1adf5dfcc7bb 104 */
Helvis 5:47262622a9bf 105 while(start == 1 && testMotion == 1) {
Helvis 0:9a3e7847a4be 106
Helvis 5:47262622a9bf 107 controller.setDesiredSpeedLeft(20.0f); //Drehzahl in [rpm]
Helvis 5:47262622a9bf 108 controller.setDesiredSpeedRight(-20.0f);
Helvis 5:47262622a9bf 109 enableMotorDriver = 1; //Schaltet den Leistungstreiber ein
Helvis 5:47262622a9bf 110
Helvis 5:47262622a9bf 111 countsLeft = counterLeft.read();
Helvis 5:47262622a9bf 112 countsRight = counterRight.read();
Helvis 5:47262622a9bf 113
Helvis 5:47262622a9bf 114 printf("countsLeft: %hd countsRight: %hd\n", countsLeft, countsRight);
Helvis 5:47262622a9bf 115
Helvis 5:47262622a9bf 116 if (start == 0) {
Helvis 5:47262622a9bf 117
Helvis 5:47262622a9bf 118 motion.stop();
Helvis 5:47262622a9bf 119 }
Helvis 1:1adf5dfcc7bb 120 }
Helvis 1:1adf5dfcc7bb 121
Helvis 1:1adf5dfcc7bb 122 /*
Helvis 1:1adf5dfcc7bb 123 Motoren Test: 180° Rotation
Helvis 1:1adf5dfcc7bb 124 */
Helvis 1:1adf5dfcc7bb 125
Helvis 5:47262622a9bf 126 while(start == 1 && testRotation == 1) {
Helvis 5:47262622a9bf 127
Helvis 5:47262622a9bf 128
Helvis 5:47262622a9bf 129 while((countsLeft - countsLeftOld) > -1614 || (countsRight - countsRightOld) > -1614) {
Helvis 1:1adf5dfcc7bb 130
Helvis 1:1adf5dfcc7bb 131 countsLeft = counterLeft.read();
Helvis 1:1adf5dfcc7bb 132 countsRight = counterRight.read();
Helvis 1:1adf5dfcc7bb 133
Helvis 5:47262622a9bf 134 controller.setDesiredSpeedLeft(-50.0f);
Helvis 1:1adf5dfcc7bb 135 controller.setDesiredSpeedRight(-50.0f);
Helvis 2:4ccdc62022c2 136 enableMotorDriver = 1;
Helvis 1:1adf5dfcc7bb 137 }
Helvis 1:1adf5dfcc7bb 138
Helvis 5:47262622a9bf 139 motion.stop();
Helvis 5:47262622a9bf 140 enableMotorDriver = 0;
Helvis 5:47262622a9bf 141 printf("countsLeft: %hd countsRight: %hd\n", countsLeft, countsRight);
Helvis 5:47262622a9bf 142
Helvis 5:47262622a9bf 143 if (start == 0) {
Helvis 5:47262622a9bf 144 countsLeftOld = countsLeft - 100;
Helvis 5:47262622a9bf 145 countsRightOld = countsRight - 100;
Helvis 5:47262622a9bf 146
Helvis 5:47262622a9bf 147 break;
Helvis 5:47262622a9bf 148 }
Helvis 5:47262622a9bf 149 }
Helvis 5:47262622a9bf 150
Helvis 5:47262622a9bf 151
Helvis 5:47262622a9bf 152 /* Benötigte Ticks für 180* Rotation am Ort:
Helvis 5:47262622a9bf 153
Helvis 5:47262622a9bf 154 mm/count = 0.122
Helvis 5:47262622a9bf 155 Dr = 124mm
Helvis 5:47262622a9bf 156 Rr = 60mm
Helvis 5:47262622a9bf 157 180°=> 1597 counts
Helvis 5:47262622a9bf 158 90°=> 798 counts
Helvis 5:47262622a9bf 159
Helvis 5:47262622a9bf 160
Helvis 5:47262622a9bf 161 RotationsDistanz Rd= DistanzRäder*pi/2 = 194.8
Helvis 5:47262622a9bf 162
Helvis 5:47262622a9bf 163 UmfangRad = Drad*pi = 188.5
Helvis 5:47262622a9bf 164
Helvis 5:47262622a9bf 165
Helvis 5:47262622a9bf 166 Counts = Rd/mm pro Count
Helvis 5:47262622a9bf 167
Helvis 5:47262622a9bf 168 => Counts pro Drehung rausfinden */
Helvis 5:47262622a9bf 169 while(start == 1 && testCounts == 1) {
Helvis 5:47262622a9bf 170
Helvis 5:47262622a9bf 171 while(countsLeft < 5000 || countsRight > -5000) {
Helvis 5:47262622a9bf 172
Helvis 5:47262622a9bf 173 countsLeft = counterLeft.read();
Helvis 5:47262622a9bf 174 countsRight = counterRight.read();
Helvis 5:47262622a9bf 175
Helvis 5:47262622a9bf 176 controller.setDesiredSpeedLeft(20.0f);
Helvis 5:47262622a9bf 177 controller.setDesiredSpeedRight(-20.0f);
Helvis 5:47262622a9bf 178 enableMotorDriver = 1;
Helvis 5:47262622a9bf 179 }
Helvis 5:47262622a9bf 180
Helvis 1:1adf5dfcc7bb 181 controller.setDesiredSpeedLeft(0.0f);
Helvis 1:1adf5dfcc7bb 182 controller.setDesiredSpeedRight(0.0f);
Helvis 4:e74c06e43485 183 enableMotorDriver = 0;
Helvis 0:9a3e7847a4be 184
Helvis 0:9a3e7847a4be 185 }
Helvis 1:1adf5dfcc7bb 186
Helvis 5:47262622a9bf 187 //Class test
Helvis 5:47262622a9bf 188 if(start == 1 && testClass == 1) {
Helvis 5:47262622a9bf 189 motion.rotate180();
Helvis 5:47262622a9bf 190 counterLeft.reset();
Helvis 5:47262622a9bf 191 counterRight.reset();
Helvis 5:47262622a9bf 192 start = 0;
Helvis 5:47262622a9bf 193 }
Helvis 1:1adf5dfcc7bb 194
Helvis 5:47262622a9bf 195 //Sensor calib
Helvis 5:47262622a9bf 196 if(start == 1) {
Helvis 5:47262622a9bf 197 int i;
Helvis 5:47262622a9bf 198 myled = !myled;
Helvis 5:47262622a9bf 199 for (i=0;i<5;i++) {
Helvis 5:47262622a9bf 200 float distanceL = irSensorL.readL();
Helvis 5:47262622a9bf 201 float distanceC = irSensorC.readC();
Helvis 5:47262622a9bf 202 float distanceR = irSensorR.readR();
Helvis 5:47262622a9bf 203 float distanceB = irSensorB.readC();
Helvis 5:47262622a9bf 204 printf("Links: %f Mitte: %f Rechts: %f Hinten: %f Line: %f\n", distanceL, distanceC, distanceR, distanceB, lineSensor.read());
Helvis 5:47262622a9bf 205 }
Helvis 5:47262622a9bf 206 printf("------------------------\n");
Helvis 5:47262622a9bf 207 start = 0;
Helvis 1:1adf5dfcc7bb 208
Helvis 5:47262622a9bf 209 /* while((countsLeft - countsLeftOld) > -82 || (countsRight - countsRightOld) < 82) {
Helvis 5:47262622a9bf 210
Helvis 5:47262622a9bf 211 controller.setDesiredSpeedLeft(-10.0f); //Drehzahl in [rpm]
Helvis 5:47262622a9bf 212 controller.setDesiredSpeedRight(10.0f);
Helvis 5:47262622a9bf 213 enableMotorDriver = 1; //Schaltet den Leistungstreiber ein
Helvis 5:47262622a9bf 214
Helvis 5:47262622a9bf 215 countsLeft = counterLeft.read();
Helvis 5:47262622a9bf 216 countsRight = counterRight.read();
Helvis 1:1adf5dfcc7bb 217 }
Helvis 1:1adf5dfcc7bb 218
Helvis 1:1adf5dfcc7bb 219 controller.setDesiredSpeedLeft(0.0f);
Helvis 1:1adf5dfcc7bb 220 controller.setDesiredSpeedRight(0.0f);
Helvis 4:e74c06e43485 221 enableMotorDriver = 0;
Helvis 1:1adf5dfcc7bb 222
Helvis 5:47262622a9bf 223 float distanceL = irSensorB.read();
Helvis 5:47262622a9bf 224 printf("Hinten: %f\n", distanceL);
Helvis 5:47262622a9bf 225
Helvis 5:47262622a9bf 226 countsLeftOld = countsLeft;
Helvis 5:47262622a9bf 227 countsRightOld = countsRight;
Helvis 5:47262622a9bf 228 start = 0;*/
Helvis 5:47262622a9bf 229
Helvis 1:1adf5dfcc7bb 230 }
Helvis 1:1adf5dfcc7bb 231 }
Helvis 5:47262622a9bf 232 }
Helvis 0:9a3e7847a4be 233
Helvis 0:9a3e7847a4be 234
Helvis 0:9a3e7847a4be 235
Helvis 0:9a3e7847a4be 236