Helvijs Kiselis
/
MicromousePES2
main.cpp: Sensoren einlesen und Motoren ansteuern
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?
User | Revision | Line number | New 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 |