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