Gruppe 3
/
PES3
hallo
Fork of PES1 by
Roboter.cpp@21:69ee872b8ee9, 2017-05-16 (annotated)
- Committer:
- itslinear
- Date:
- Tue May 16 16:09:08 2017 +0000
- Revision:
- 21:69ee872b8ee9
- Parent:
- 20:a90e5b54bf7b
sieg
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
itslinear | 0:306a2438de17 | 1 | #include "Roboter.h" |
itslinear | 0:306a2438de17 | 2 | |
itslinear | 0:306a2438de17 | 3 | |
itslinear | 18:3ee1b02ed3aa | 4 | Roboter::Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR, Servo* servo1, Servo* servo2, EncoderCounter* counterLeft, EncoderCounter* counterRight, DigitalOut* enableMotorDriver) |
itslinear | 4:fdcf3b5009c6 | 5 | |
itslinear | 1:4e84271a70c6 | 6 | { |
itslinear | 1:4e84271a70c6 | 7 | sensors[0].init( distance, bit0, bit1, bit2, 0); |
itslinear | 1:4e84271a70c6 | 8 | sensors[1].init( distance, bit0, bit1, bit2, 1); |
itslinear | 1:4e84271a70c6 | 9 | sensors[2].init( distance, bit0, bit1, bit2, 2); |
itslinear | 1:4e84271a70c6 | 10 | sensors[3].init( distance, bit0, bit1, bit2, 3); |
itslinear | 1:4e84271a70c6 | 11 | sensors[4].init( distance, bit0, bit1, bit2, 4); |
itslinear | 1:4e84271a70c6 | 12 | sensors[5].init( distance, bit0, bit1, bit2, 5); |
itslinear | 0:306a2438de17 | 13 | |
itslinear | 1:4e84271a70c6 | 14 | this->pwmR = pwmR; |
itslinear | 1:4e84271a70c6 | 15 | this->pwmL = pwmL; |
itslinear | 18:3ee1b02ed3aa | 16 | this->servo1 = servo1; |
itslinear | 18:3ee1b02ed3aa | 17 | this->servo2 = servo2; |
itslinear | 18:3ee1b02ed3aa | 18 | this->counterLeft = counterLeft; |
itslinear | 18:3ee1b02ed3aa | 19 | this->counterRight = counterRight; |
itslinear | 18:3ee1b02ed3aa | 20 | this->enableMotorDriver = enableMotorDriver; |
itslinear | 4:fdcf3b5009c6 | 21 | |
itslinear | 18:3ee1b02ed3aa | 22 | initSpeedcontrol(); |
itslinear | 18:3ee1b02ed3aa | 23 | |
itslinear | 18:3ee1b02ed3aa | 24 | t1.attach(this, &Roboter::speedCtrl, PERIOD); |
itslinear | 1:4e84271a70c6 | 25 | |
itslinear | 1:4e84271a70c6 | 26 | } |
itslinear | 1:4e84271a70c6 | 27 | |
itslinear | 19:adae367247d4 | 28 | int Roboter::readSensors() // Sensoren auslesen |
itslinear | 0:306a2438de17 | 29 | { |
itslinear | 20:a90e5b54bf7b | 30 | int lr = 1; |
itslinear | 21:69ee872b8ee9 | 31 | float x = 0.1; // Distanz ab welcher sensoren reagieren sollen |
itslinear | 19:adae367247d4 | 32 | |
itslinear | 19:adae367247d4 | 33 | if(sensors[1] < x || sensors[5] < x) { // Sensor rechts und links |
itslinear | 20:a90e5b54bf7b | 34 | lr = 6; |
itslinear | 19:adae367247d4 | 35 | } |
itslinear | 20:a90e5b54bf7b | 36 | return lr; |
itslinear | 1:4e84271a70c6 | 37 | |
itslinear | 0:306a2438de17 | 38 | } |
itslinear | 0:306a2438de17 | 39 | |
itslinear | 1:4e84271a70c6 | 40 | |
itslinear | 19:adae367247d4 | 41 | void Roboter::ausweichen1() // Hindernissen ausweichen |
itslinear | 0:306a2438de17 | 42 | { |
itslinear | 21:69ee872b8ee9 | 43 | float x = 0.14; // Distanz ab welcher sensoren reagieren sollen |
itslinear | 1:4e84271a70c6 | 44 | |
itslinear | 21:69ee872b8ee9 | 45 | while(sensors[0] < (x-0.01f) && sensors[1] < x && sensors[5] < x) { // alle sensoren aktiv, roboter fährt nach hinten |
itslinear | 21:69ee872b8ee9 | 46 | desiredSpeedLeft = -v; |
itslinear | 21:69ee872b8ee9 | 47 | desiredSpeedRight = v*4; |
itslinear | 9:d9e46f9c9e40 | 48 | |
itslinear | 9:d9e46f9c9e40 | 49 | } |
itslinear | 9:d9e46f9c9e40 | 50 | |
itslinear | 21:69ee872b8ee9 | 51 | while(sensors[0] < x) { // sensor vorne, roboter dreht nach links |
itslinear | 20:a90e5b54bf7b | 52 | desiredSpeedLeft = -v*2; |
itslinear | 20:a90e5b54bf7b | 53 | desiredSpeedRight = -v*2; |
itslinear | 9:d9e46f9c9e40 | 54 | |
itslinear | 6:4af735d26b7a | 55 | } |
itslinear | 0:306a2438de17 | 56 | |
itslinear | 9:d9e46f9c9e40 | 57 | while(sensors[1] < x) { // sensor rechts, roboter dreht nach links |
itslinear | 20:a90e5b54bf7b | 58 | desiredSpeedLeft = -v*2; |
itslinear | 20:a90e5b54bf7b | 59 | desiredSpeedRight = -v*2; |
itslinear | 9:d9e46f9c9e40 | 60 | |
itslinear | 1:4e84271a70c6 | 61 | } |
itslinear | 1:4e84271a70c6 | 62 | |
itslinear | 21:69ee872b8ee9 | 63 | while(sensors[5] < x) { // sensor links, roboter dreht nach rechts |
itslinear | 20:a90e5b54bf7b | 64 | desiredSpeedLeft = v*2; |
itslinear | 20:a90e5b54bf7b | 65 | desiredSpeedRight = v*2; |
itslinear | 9:d9e46f9c9e40 | 66 | |
itslinear | 9:d9e46f9c9e40 | 67 | } |
itslinear | 1:4e84271a70c6 | 68 | |
itslinear | 3:24e098715b78 | 69 | } |
schneju2 | 2:953263712480 | 70 | |
itslinear | 19:adae367247d4 | 71 | int Roboter::ausweichen2() // Hindernissen ausweichen beim Anfahren des Klotzes |
itslinear | 19:adae367247d4 | 72 | { |
itslinear | 21:69ee872b8ee9 | 73 | float x = 0.1; // Distanz ab welcher sensoren reagieren sollen |
itslinear | 19:adae367247d4 | 74 | static int l = 0; |
itslinear | 19:adae367247d4 | 75 | static int r = 0; |
itslinear | 19:adae367247d4 | 76 | static int ii = 0; |
itslinear | 19:adae367247d4 | 77 | int s = 6; |
itslinear | 19:adae367247d4 | 78 | |
itslinear | 19:adae367247d4 | 79 | if(sensors[1] < x && l == 0) { |
itslinear | 19:adae367247d4 | 80 | r = 1; |
itslinear | 19:adae367247d4 | 81 | } |
itslinear | 19:adae367247d4 | 82 | |
itslinear | 19:adae367247d4 | 83 | if(sensors[5] < x && r == 0) { |
itslinear | 19:adae367247d4 | 84 | l = 1; |
itslinear | 19:adae367247d4 | 85 | } |
itslinear | 19:adae367247d4 | 86 | |
itslinear | 19:adae367247d4 | 87 | if(r == 1) { |
itslinear | 21:69ee872b8ee9 | 88 | if(ii < 500) { // rückwärts fahren |
itslinear | 19:adae367247d4 | 89 | desiredSpeedLeft = -(v+30); |
itslinear | 20:a90e5b54bf7b | 90 | desiredSpeedRight = v+30; |
itslinear | 19:adae367247d4 | 91 | } |
itslinear | 21:69ee872b8ee9 | 92 | if(ii > 499 && ii < 900) { // links drehen |
itslinear | 20:a90e5b54bf7b | 93 | desiredSpeedLeft = -v*2; |
itslinear | 20:a90e5b54bf7b | 94 | desiredSpeedRight = -v*2; |
itslinear | 20:a90e5b54bf7b | 95 | } |
itslinear | 21:69ee872b8ee9 | 96 | if(ii > 899 && ii < 1800) { // gerade fahren |
itslinear | 19:adae367247d4 | 97 | desiredSpeedLeft = v*2; |
itslinear | 19:adae367247d4 | 98 | desiredSpeedRight = -v*2; |
itslinear | 19:adae367247d4 | 99 | } |
itslinear | 21:69ee872b8ee9 | 100 | if(ii > 1799 && ii < 2300) { // rechts drehen |
itslinear | 20:a90e5b54bf7b | 101 | desiredSpeedLeft = v*2; |
itslinear | 20:a90e5b54bf7b | 102 | desiredSpeedRight = v*2; |
itslinear | 19:adae367247d4 | 103 | } |
itslinear | 19:adae367247d4 | 104 | ii++; |
itslinear | 21:69ee872b8ee9 | 105 | if(ii > 2299) { // rücksetzen |
itslinear | 19:adae367247d4 | 106 | desiredSpeedLeft = 0; |
itslinear | 19:adae367247d4 | 107 | desiredSpeedRight = 0; |
itslinear | 19:adae367247d4 | 108 | r = 0; |
itslinear | 19:adae367247d4 | 109 | ii = 0; |
itslinear | 19:adae367247d4 | 110 | s = 1; |
itslinear | 19:adae367247d4 | 111 | } |
itslinear | 19:adae367247d4 | 112 | } |
itslinear | 19:adae367247d4 | 113 | |
itslinear | 19:adae367247d4 | 114 | if(l == 1) { |
itslinear | 20:a90e5b54bf7b | 115 | if(ii < 500) { // rückwärts fahren |
itslinear | 20:a90e5b54bf7b | 116 | desiredSpeedLeft = -(v+30); |
itslinear | 19:adae367247d4 | 117 | desiredSpeedRight = v+30; |
itslinear | 19:adae367247d4 | 118 | } |
itslinear | 21:69ee872b8ee9 | 119 | if(ii > 499 && ii < 900) { // rechts drehen |
itslinear | 20:a90e5b54bf7b | 120 | desiredSpeedLeft = v*2; |
itslinear | 20:a90e5b54bf7b | 121 | desiredSpeedRight = v*2; |
itslinear | 20:a90e5b54bf7b | 122 | } |
itslinear | 21:69ee872b8ee9 | 123 | if(ii > 899 && ii < 1800) { // gerade fahren |
itslinear | 19:adae367247d4 | 124 | desiredSpeedLeft = v*2; |
itslinear | 19:adae367247d4 | 125 | desiredSpeedRight = -v*2; |
itslinear | 19:adae367247d4 | 126 | } |
itslinear | 21:69ee872b8ee9 | 127 | if(ii > 1799 && ii < 2300) { // links drehen |
itslinear | 20:a90e5b54bf7b | 128 | desiredSpeedLeft = -v*2; |
itslinear | 20:a90e5b54bf7b | 129 | desiredSpeedRight = -v*2; |
itslinear | 19:adae367247d4 | 130 | } |
itslinear | 19:adae367247d4 | 131 | ii++; |
itslinear | 21:69ee872b8ee9 | 132 | if(ii > 2299) { |
itslinear | 19:adae367247d4 | 133 | desiredSpeedLeft = 0; |
itslinear | 19:adae367247d4 | 134 | desiredSpeedRight = 0; |
itslinear | 19:adae367247d4 | 135 | l = 0; |
itslinear | 19:adae367247d4 | 136 | ii = 0; |
itslinear | 19:adae367247d4 | 137 | s = 1; |
itslinear | 19:adae367247d4 | 138 | } |
itslinear | 19:adae367247d4 | 139 | } |
itslinear | 19:adae367247d4 | 140 | return s; |
itslinear | 19:adae367247d4 | 141 | } |
itslinear | 19:adae367247d4 | 142 | |
schneju2 | 2:953263712480 | 143 | |
itslinear | 18:3ee1b02ed3aa | 144 | int Roboter::pickup() // Klotz aufnehmen |
itslinear | 18:3ee1b02ed3aa | 145 | { |
itslinear | 18:3ee1b02ed3aa | 146 | static int ii = 0; |
itslinear | 18:3ee1b02ed3aa | 147 | int r; // Rückegabewert |
itslinear | 18:3ee1b02ed3aa | 148 | desiredSpeedLeft = 0; |
itslinear | 18:3ee1b02ed3aa | 149 | desiredSpeedRight = 0; |
schneju2 | 2:953263712480 | 150 | |
itslinear | 21:69ee872b8ee9 | 151 | if(ii < 650) { // Arm nach unten fahren |
itslinear | 18:3ee1b02ed3aa | 152 | servo2->SetPosition(2250); |
itslinear | 18:3ee1b02ed3aa | 153 | } |
itslinear | 20:a90e5b54bf7b | 154 | if(ii > 649 && ii < 1100) { // gerade fahren |
itslinear | 18:3ee1b02ed3aa | 155 | desiredSpeedLeft = v; |
itslinear | 18:3ee1b02ed3aa | 156 | desiredSpeedRight = -v; |
itslinear | 18:3ee1b02ed3aa | 157 | } |
itslinear | 20:a90e5b54bf7b | 158 | if(ii > 1099 && ii < 1500) { // gerade fahren + Greifer schliessen |
itslinear | 20:a90e5b54bf7b | 159 | if(ii < 1400) { |
itslinear | 20:a90e5b54bf7b | 160 | desiredSpeedLeft = v; |
itslinear | 20:a90e5b54bf7b | 161 | desiredSpeedRight = -v; |
itslinear | 20:a90e5b54bf7b | 162 | } |
itslinear | 18:3ee1b02ed3aa | 163 | servo1->SetPosition(500); |
itslinear | 18:3ee1b02ed3aa | 164 | } |
itslinear | 20:a90e5b54bf7b | 165 | if(ii > 1499 && ii < 1900) { // rückwärts fahren |
itslinear | 20:a90e5b54bf7b | 166 | desiredSpeedLeft = -v; |
itslinear | 20:a90e5b54bf7b | 167 | desiredSpeedRight = v; |
itslinear | 19:adae367247d4 | 168 | } |
itslinear | 20:a90e5b54bf7b | 169 | if(ii > 1899 && ii < 2550) { // Arm nach oben fahren |
itslinear | 18:3ee1b02ed3aa | 170 | desiredSpeedLeft = 0; |
itslinear | 18:3ee1b02ed3aa | 171 | desiredSpeedRight = 0; |
itslinear | 18:3ee1b02ed3aa | 172 | servo2->SetPosition(700); |
itslinear | 18:3ee1b02ed3aa | 173 | } |
itslinear | 20:a90e5b54bf7b | 174 | if(ii > 2549 && ii < 3000) { // Greifer öffnen |
itslinear | 18:3ee1b02ed3aa | 175 | servo1->SetPosition(1500); |
itslinear | 18:3ee1b02ed3aa | 176 | } |
itslinear | 20:a90e5b54bf7b | 177 | |
itslinear | 18:3ee1b02ed3aa | 178 | ii++; |
itslinear | 21:69ee872b8ee9 | 179 | r = 5; // Rückegabewert |
itslinear | 18:3ee1b02ed3aa | 180 | |
itslinear | 20:a90e5b54bf7b | 181 | if( ii > 2999 ) { |
itslinear | 18:3ee1b02ed3aa | 182 | r = 1; |
itslinear | 18:3ee1b02ed3aa | 183 | ii = 0; |
itslinear | 18:3ee1b02ed3aa | 184 | } |
itslinear | 20:a90e5b54bf7b | 185 | |
itslinear | 18:3ee1b02ed3aa | 186 | return r; |
itslinear | 18:3ee1b02ed3aa | 187 | } |
itslinear | 1:4e84271a70c6 | 188 | |
itslinear | 18:3ee1b02ed3aa | 189 | void Roboter::anfahren(float cam) |
itslinear | 18:3ee1b02ed3aa | 190 | { |
itslinear | 18:3ee1b02ed3aa | 191 | float speedValue; |
itslinear | 18:3ee1b02ed3aa | 192 | float maxWert = 20; |
itslinear | 18:3ee1b02ed3aa | 193 | if(cam == 100) { |
itslinear | 18:3ee1b02ed3aa | 194 | desiredSpeedLeft = -v/2; |
itslinear | 18:3ee1b02ed3aa | 195 | desiredSpeedRight = -v/2; |
itslinear | 18:3ee1b02ed3aa | 196 | } |
itslinear | 18:3ee1b02ed3aa | 197 | if(cam == 200) { |
itslinear | 18:3ee1b02ed3aa | 198 | desiredSpeedLeft = v/2; |
itslinear | 18:3ee1b02ed3aa | 199 | desiredSpeedRight = v/2; |
itslinear | 18:3ee1b02ed3aa | 200 | } |
itslinear | 18:3ee1b02ed3aa | 201 | if(cam > 100 && cam <200 ) { // links fahren, wenn wir Werte von 100 bis 199 haben |
itslinear | 18:3ee1b02ed3aa | 202 | cam = cam -99.0f; // cam nimmt die Werte von 1 bis 100 an |
itslinear | 20:a90e5b54bf7b | 203 | speedValue = (v) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's |
itslinear | 18:3ee1b02ed3aa | 204 | desiredSpeedLeft = -speedValue; |
itslinear | 18:3ee1b02ed3aa | 205 | desiredSpeedRight = -speedValue; |
itslinear | 18:3ee1b02ed3aa | 206 | } |
itslinear | 18:3ee1b02ed3aa | 207 | if(cam > 200 && cam < 300) { // rechts fahren, wenn wir Werte von 200 bis 299 haben |
itslinear | 18:3ee1b02ed3aa | 208 | cam = cam -199.0f; // cam nimmt die Werte von 1 bis 100 an |
itslinear | 20:a90e5b54bf7b | 209 | speedValue = (v) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's |
itslinear | 18:3ee1b02ed3aa | 210 | desiredSpeedLeft = speedValue; |
itslinear | 18:3ee1b02ed3aa | 211 | desiredSpeedRight = speedValue; |
itslinear | 18:3ee1b02ed3aa | 212 | } |
itslinear | 18:3ee1b02ed3aa | 213 | if(cam >= 300 && cam <400) { // gerade fahren, wenn wir Werte von 300 bis 399 haben |
itslinear | 18:3ee1b02ed3aa | 214 | cam = cam-299.0f; // cam nimmt die Werte von 1 bis 100 an |
itslinear | 20:a90e5b54bf7b | 215 | speedValue = (v*3) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's |
itslinear | 18:3ee1b02ed3aa | 216 | desiredSpeedLeft = speedValue; |
itslinear | 18:3ee1b02ed3aa | 217 | desiredSpeedRight = -speedValue; |
itslinear | 18:3ee1b02ed3aa | 218 | } |
itslinear | 18:3ee1b02ed3aa | 219 | } |
itslinear | 18:3ee1b02ed3aa | 220 | |
itslinear | 18:3ee1b02ed3aa | 221 | int Roboter::geradeFahren() |
itslinear | 3:24e098715b78 | 222 | { |
itslinear | 18:3ee1b02ed3aa | 223 | int r; |
itslinear | 18:3ee1b02ed3aa | 224 | static int time2 = 0; |
itslinear | 21:69ee872b8ee9 | 225 | if(time2 < 25) { // 1s gerade aus fahren |
itslinear | 20:a90e5b54bf7b | 226 | desiredSpeedLeft = v*4; |
itslinear | 20:a90e5b54bf7b | 227 | desiredSpeedRight = -v*4; |
itslinear | 18:3ee1b02ed3aa | 228 | time2 ++; |
itslinear | 18:3ee1b02ed3aa | 229 | r = 1; // state |
itslinear | 18:3ee1b02ed3aa | 230 | } else { |
itslinear | 18:3ee1b02ed3aa | 231 | time2 = 0; |
itslinear | 18:3ee1b02ed3aa | 232 | desiredSpeedLeft = 0.0f; |
itslinear | 18:3ee1b02ed3aa | 233 | desiredSpeedRight = 0.0f; |
itslinear | 18:3ee1b02ed3aa | 234 | r = 2; //state |
itslinear | 18:3ee1b02ed3aa | 235 | } |
itslinear | 18:3ee1b02ed3aa | 236 | return r; |
itslinear | 18:3ee1b02ed3aa | 237 | } |
itslinear | 3:24e098715b78 | 238 | |
itslinear | 19:adae367247d4 | 239 | int Roboter::back() |
itslinear | 19:adae367247d4 | 240 | { |
itslinear | 19:adae367247d4 | 241 | static int ii = 0; |
itslinear | 20:a90e5b54bf7b | 242 | int s = 7; |
itslinear | 19:adae367247d4 | 243 | |
itslinear | 20:a90e5b54bf7b | 244 | if(ii < 1000) { |
itslinear | 19:adae367247d4 | 245 | desiredSpeedLeft = -v; |
itslinear | 19:adae367247d4 | 246 | desiredSpeedRight = v; |
itslinear | 19:adae367247d4 | 247 | } |
itslinear | 20:a90e5b54bf7b | 248 | |
itslinear | 21:69ee872b8ee9 | 249 | if(ii > 999 && ii < 1800) { |
itslinear | 20:a90e5b54bf7b | 250 | desiredSpeedLeft = v*2; |
itslinear | 20:a90e5b54bf7b | 251 | desiredSpeedRight = v*2; |
itslinear | 20:a90e5b54bf7b | 252 | } |
itslinear | 20:a90e5b54bf7b | 253 | |
itslinear | 19:adae367247d4 | 254 | ii++; |
itslinear | 19:adae367247d4 | 255 | |
itslinear | 21:69ee872b8ee9 | 256 | if(ii > 1799) { |
itslinear | 19:adae367247d4 | 257 | desiredSpeedLeft = 0; |
itslinear | 19:adae367247d4 | 258 | desiredSpeedRight = 0; |
itslinear | 19:adae367247d4 | 259 | ii = 0; |
itslinear | 19:adae367247d4 | 260 | s = 1; |
itslinear | 19:adae367247d4 | 261 | } |
itslinear | 20:a90e5b54bf7b | 262 | |
itslinear | 19:adae367247d4 | 263 | return s; |
itslinear | 19:adae367247d4 | 264 | } |
itslinear | 19:adae367247d4 | 265 | |
itslinear | 18:3ee1b02ed3aa | 266 | int Roboter::kreisFahren() |
itslinear | 18:3ee1b02ed3aa | 267 | { |
itslinear | 18:3ee1b02ed3aa | 268 | int r; |
itslinear | 21:69ee872b8ee9 | 269 | static int anzahl = 0; |
itslinear | 18:3ee1b02ed3aa | 270 | static int time1 = 0; |
itslinear | 21:69ee872b8ee9 | 271 | |
itslinear | 21:69ee872b8ee9 | 272 | if(time1 < 6 && anzahl%2 == 0) { // 1s im Kreis drehen |
itslinear | 21:69ee872b8ee9 | 273 | desiredSpeedLeft = v*2; |
itslinear | 21:69ee872b8ee9 | 274 | desiredSpeedRight = v*2; |
itslinear | 18:3ee1b02ed3aa | 275 | time1 ++; |
itslinear | 18:3ee1b02ed3aa | 276 | r = 1; // state |
itslinear | 18:3ee1b02ed3aa | 277 | } else { |
itslinear | 21:69ee872b8ee9 | 278 | desiredSpeedLeft = -(v*2); |
itslinear | 21:69ee872b8ee9 | 279 | desiredSpeedRight = -(v*2); |
itslinear | 21:69ee872b8ee9 | 280 | time1 ++; |
itslinear | 21:69ee872b8ee9 | 281 | r = 1; // state |
itslinear | 21:69ee872b8ee9 | 282 | } |
itslinear | 21:69ee872b8ee9 | 283 | if(time1 == 6) { |
itslinear | 18:3ee1b02ed3aa | 284 | time1 = 0; |
itslinear | 21:69ee872b8ee9 | 285 | anzahl ++; |
itslinear | 18:3ee1b02ed3aa | 286 | desiredSpeedLeft = 0.0f; |
itslinear | 18:3ee1b02ed3aa | 287 | desiredSpeedRight = 0.0f; |
itslinear | 18:3ee1b02ed3aa | 288 | r = 3; // state |
itslinear | 18:3ee1b02ed3aa | 289 | } |
itslinear | 18:3ee1b02ed3aa | 290 | return r; |
itslinear | 18:3ee1b02ed3aa | 291 | } |
itslinear | 18:3ee1b02ed3aa | 292 | |
itslinear | 20:a90e5b54bf7b | 293 | int Roboter::stateAuswahl(float cam, int tempState, int temp) |
itslinear | 18:3ee1b02ed3aa | 294 | { |
itslinear | 20:a90e5b54bf7b | 295 | int s = 1; |
itslinear | 18:3ee1b02ed3aa | 296 | if(cam >= 100.0f && cam < 400.0f ) { // Die Kamera erkennt ein grünen Klotz , Werte von 100 und 399 |
itslinear | 18:3ee1b02ed3aa | 297 | s = 4; |
itslinear | 18:3ee1b02ed3aa | 298 | } |
itslinear | 20:a90e5b54bf7b | 299 | if(cam == 400.0f && temp != 5) { // Roboter in Position |
itslinear | 18:3ee1b02ed3aa | 300 | s = 5; |
itslinear | 18:3ee1b02ed3aa | 301 | } |
itslinear | 21:69ee872b8ee9 | 302 | if(cam == 400.0f && temp == 5) { // Roboter immer noch beim selben Klotz -> back() |
itslinear | 20:a90e5b54bf7b | 303 | s = 7; |
itslinear | 20:a90e5b54bf7b | 304 | } |
itslinear | 20:a90e5b54bf7b | 305 | if(cam < 1) { |
itslinear | 20:a90e5b54bf7b | 306 | s = tempState; |
itslinear | 18:3ee1b02ed3aa | 307 | } |
itslinear | 19:adae367247d4 | 308 | if(cam == 1) { |
itslinear | 19:adae367247d4 | 309 | s = 1; |
itslinear | 19:adae367247d4 | 310 | } |
itslinear | 18:3ee1b02ed3aa | 311 | return s; |
itslinear | 0:306a2438de17 | 312 | |
schneju2 | 2:953263712480 | 313 | |
itslinear | 3:24e098715b78 | 314 | } |
itslinear | 18:3ee1b02ed3aa | 315 | |
itslinear | 18:3ee1b02ed3aa | 316 | void Roboter::initSpeedcontrol() |
itslinear | 18:3ee1b02ed3aa | 317 | { |
itslinear | 18:3ee1b02ed3aa | 318 | // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us |
itslinear | 18:3ee1b02ed3aa | 319 | pwmL->period(0.00005f); // Setzt die Periode auf 50 μs |
itslinear | 18:3ee1b02ed3aa | 320 | pwmR->period(0.00005f); |
itslinear | 18:3ee1b02ed3aa | 321 | *pwmL = 0.5f; // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us |
itslinear | 18:3ee1b02ed3aa | 322 | *pwmR = 0.5f; // Duty-Cycle von 50% |
itslinear | 18:3ee1b02ed3aa | 323 | |
itslinear | 18:3ee1b02ed3aa | 324 | // Initialisieren von lokalen Variabeln |
itslinear | 18:3ee1b02ed3aa | 325 | previousValueCounterLeft = counterLeft->read(); |
itslinear | 18:3ee1b02ed3aa | 326 | previousValueCounterRight = counterRight->read(); |
itslinear | 18:3ee1b02ed3aa | 327 | speedLeftFilter.setPeriod(PERIOD); |
itslinear | 18:3ee1b02ed3aa | 328 | speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY); |
itslinear | 18:3ee1b02ed3aa | 329 | speedRightFilter.setPeriod(PERIOD); |
itslinear | 18:3ee1b02ed3aa | 330 | speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY); |
itslinear | 18:3ee1b02ed3aa | 331 | |
itslinear | 18:3ee1b02ed3aa | 332 | desiredSpeedLeft = 0.0f; |
itslinear | 18:3ee1b02ed3aa | 333 | desiredSpeedRight = 0.0f; |
itslinear | 18:3ee1b02ed3aa | 334 | actualSpeedLeft = 0.0f; |
itslinear | 18:3ee1b02ed3aa | 335 | actualSpeedRight = 0.0f; |
itslinear | 18:3ee1b02ed3aa | 336 | |
itslinear | 18:3ee1b02ed3aa | 337 | *enableMotorDriver = 1; |
itslinear | 18:3ee1b02ed3aa | 338 | } |
itslinear | 18:3ee1b02ed3aa | 339 | |
itslinear | 18:3ee1b02ed3aa | 340 | void Roboter::speedCtrl() |
itslinear | 18:3ee1b02ed3aa | 341 | { |
itslinear | 18:3ee1b02ed3aa | 342 | // Berechnen die effektiven Drehzahlen der Motoren in [rpm] |
itslinear | 18:3ee1b02ed3aa | 343 | short valueCounterLeft = counterLeft->read(); |
itslinear | 18:3ee1b02ed3aa | 344 | short valueCounterRight = counterRight->read(); |
itslinear | 18:3ee1b02ed3aa | 345 | short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft; |
itslinear | 18:3ee1b02ed3aa | 346 | short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight; |
itslinear | 18:3ee1b02ed3aa | 347 | |
itslinear | 18:3ee1b02ed3aa | 348 | previousValueCounterLeft = valueCounterLeft; |
itslinear | 18:3ee1b02ed3aa | 349 | previousValueCounterRight = valueCounterRight; |
itslinear | 18:3ee1b02ed3aa | 350 | actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft /COUNTS_PER_TURN/PERIOD*60.0f); |
itslinear | 18:3ee1b02ed3aa | 351 | actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight /COUNTS_PER_TURN/PERIOD*60.0f); |
itslinear | 18:3ee1b02ed3aa | 352 | |
itslinear | 18:3ee1b02ed3aa | 353 | // Berechnen der Motorspannungen Uout |
itslinear | 18:3ee1b02ed3aa | 354 | float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN; |
itslinear | 18:3ee1b02ed3aa | 355 | float voltageRight = KP*(desiredSpeedRight*correction-actualSpeedRight)+desiredSpeedRight*correction/KN; |
itslinear | 18:3ee1b02ed3aa | 356 | |
itslinear | 18:3ee1b02ed3aa | 357 | // Berechnen, Limitieren und Setzen der Duty-Cycle |
itslinear | 18:3ee1b02ed3aa | 358 | float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE; |
itslinear | 18:3ee1b02ed3aa | 359 | if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE; |
itslinear | 18:3ee1b02ed3aa | 360 | else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE; |
itslinear | 18:3ee1b02ed3aa | 361 | |
itslinear | 18:3ee1b02ed3aa | 362 | *pwmL = dutyCycleLeft; |
itslinear | 18:3ee1b02ed3aa | 363 | float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE; |
itslinear | 18:3ee1b02ed3aa | 364 | if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE; |
itslinear | 18:3ee1b02ed3aa | 365 | else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE; |
itslinear | 18:3ee1b02ed3aa | 366 | |
itslinear | 18:3ee1b02ed3aa | 367 | *pwmR = dutyCycleRight; |
itslinear | 18:3ee1b02ed3aa | 368 | } |