project enzo / Mbed 2 deprecated robot Featured

dit is em

Dependencies:   mbed

main.cpp@20:9f4ba1b3d06e, 2017-05-31 (annotated)

Committer:
joosthartkamp
Date:
Wed May 31 08:43:43 2017 +0000
Revision:
20:9f4ba1b3d06e
Parent:
19:25663276160d 
ongetest;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bjorntukkertje 0:9756a10feb5f 1 #include "mbed.h"
bjorntukkertje 0:9756a10feb5f 2
bjorntukkertje 19:25663276160d 3
joosthartkamp 18:3a613ea8e28c 4 PwmOut motorleftspeed(D6);
joosthartkamp 16:a9382753e8d6 5 PwmOut motorrightspeed(D7);
joosthartkamp 18:3a613ea8e28c 6 DigitalOut MotorL1pin (D12);
joosthartkamp 18:3a613ea8e28c 7 DigitalOut MotorL2pin (D11);
joosthartkamp 18:3a613ea8e28c 8 DigitalOut MotorR1pin (D10);
joosthartkamp 18:3a613ea8e28c 9 DigitalOut MotorR2pin (D9);
bjorntukkertje 6:6659a8ada914 10 AnalogIn Sensor1 (A1);
bjorntukkertje 6:6659a8ada914 11 AnalogIn Sensor2 (A2);
bjorntukkertje 6:6659a8ada914 12 AnalogIn Sensor3 (A3);
bjorntukkertje 6:6659a8ada914 13 AnalogIn Sensor4 (A4);
bjorntukkertje 11:aac90fca0290 14 AnalogIn zoeksensor (A5);
joosthartkamp 16:a9382753e8d6 15 AnalogIn currentleftpin (A6);
joosthartkamp 16:a9382753e8d6 16 AnalogIn currentrightpin (A7);
bjorntukkertje 10:18116d979d61 17 DigitalOut steppera (D2);
bjorntukkertje 10:18116d979d61 18 DigitalOut stepperb (D3);
bjorntukkertje 10:18116d979d61 19 DigitalOut stepperc (D4);
bjorntukkertje 10:18116d979d61 20 DigitalOut stepperd (D5);
joosthartkamp 18:3a613ea8e28c 21 DigitalIn knop(D8);
joosthartkamp 18:3a613ea8e28c 22 DigitalOut led(D1);
bjorntukkertje 0:9756a10feb5f 23
bjorntukkertje 0:9756a10feb5f 24
bjorntukkertje 11:aac90fca0290 25 int afstand = zoeksensor;
bjorntukkertje 11:aac90fca0290 26
bjorntukkertje 10:18116d979d61 27 int stapmode = 0;
bjorntukkertje 10:18116d979d61 28 int position = 0;
bjorntukkertje 10:18116d979d61 29 int pos;
bjorntukkertje 10:18116d979d61 30
joosthartkamp 20:9f4ba1b3d06e 31 bool robotrun;
joosthartkamp 20:9f4ba1b3d06e 32 int radarspeedsetleft;
joosthartkamp 20:9f4ba1b3d06e 33 int radarspeedsetright;
bjorntukkertje 19:25663276160d 34
joosthartkamp 20:9f4ba1b3d06e 35
joosthartkamp 20:9f4ba1b3d06e 36 /*
joosthartkamp 20:9f4ba1b3d06e 37 ******* main loop ********************************
joosthartkamp 20:9f4ba1b3d06e 38 hier maakt de robot de beslissingen wat hij moet doen in bepaalde situaties
joosthartkamp 20:9f4ba1b3d06e 39 dit staat in het programmaoverzicht beschreven
joosthartkamp 20:9f4ba1b3d06e 40
joosthartkamp 20:9f4ba1b3d06e 41
joosthartkamp 20:9f4ba1b3d06e 42 */
joosthartkamp 20:9f4ba1b3d06e 43 int main ()
joosthartkamp 20:9f4ba1b3d06e 44 {
joosthartkamp 20:9f4ba1b3d06e 45 while(robotrun == 1);
joosthartkamp 20:9f4ba1b3d06e 46 {
joosthartkamp 20:9f4ba1b3d06e 47 if(lijnsensor > 0) {
joosthartkamp 20:9f4ba1b3d06e 48 switch(lijnsensor) {
joosthartkamp 20:9f4ba1b3d06e 49 case 1: {
joosthartkamp 20:9f4ba1b3d06e 50 Hbrug(255, 128, 2);
joosthartkamp 20:9f4ba1b3d06e 51 break;
joosthartkamp 20:9f4ba1b3d06e 52 }
joosthartkamp 20:9f4ba1b3d06e 53 case 2: {
joosthartkamp 20:9f4ba1b3d06e 54 Hbrug(128, 255, 2);
joosthartkamp 20:9f4ba1b3d06e 55 break;
joosthartkamp 20:9f4ba1b3d06e 56 }
joosthartkamp 20:9f4ba1b3d06e 57 case 3: {
joosthartkamp 20:9f4ba1b3d06e 58 Hbrug(255, 255, 2);
joosthartkamp 20:9f4ba1b3d06e 59 break;
joosthartkamp 20:9f4ba1b3d06e 60 }
joosthartkamp 20:9f4ba1b3d06e 61 case 4: {
joosthartkamp 20:9f4ba1b3d06e 62 Hbrug(255, 128, 1);
joosthartkamp 20:9f4ba1b3d06e 63 break;
joosthartkamp 20:9f4ba1b3d06e 64 }
joosthartkamp 20:9f4ba1b3d06e 65 case 8: {
joosthartkamp 20:9f4ba1b3d06e 66 Hbrug(128, 255, 1);
joosthartkamp 20:9f4ba1b3d06e 67 break;
joosthartkamp 20:9f4ba1b3d06e 68 }
joosthartkamp 20:9f4ba1b3d06e 69 case 12: {
joosthartkamp 20:9f4ba1b3d06e 70 Hbrug(255, 255, 1);
joosthartkamp 20:9f4ba1b3d06e 71 break;
joosthartkamp 20:9f4ba1b3d06e 72 }
joosthartkamp 20:9f4ba1b3d06e 73 wait_ms(200);
joosthartkamp 20:9f4ba1b3d06e 74 }
joosthartkamp 20:9f4ba1b3d06e 75 else {
joosthartkamp 20:9f4ba1b3d06e 76 if (lidardirection < 90) {
joosthartkamp 20:9f4ba1b3d06e 77 radarspeedsetleft = 255;
joosthartkamp 20:9f4ba1b3d06e 78 radarspeedsetright = 255-(lidardirection/90)*255;
joosthartkamp 20:9f4ba1b3d06e 79 Hbrug(radarspeedsetleft, radarspeedsetright, 1);
joosthartkamp 20:9f4ba1b3d06e 80 }
joosthartkamp 20:9f4ba1b3d06e 81 else if (lidardirection > 270) {
joosthartkamp 20:9f4ba1b3d06e 82 radarspeedsetleft = 255;
joosthartkamp 20:9f4ba1b3d06e 83 radarspeedsetright = ((lidardirection-270)/90)*255;
joosthartkamp 20:9f4ba1b3d06e 84 Hbrug(radarspeedsetleft, radarspeedsetright, 1);
joosthartkamp 20:9f4ba1b3d06e 85 }
joosthartkamp 20:9f4ba1b3d06e 86 else if (lidardirection > 270) {
joosthartkamp 20:9f4ba1b3d06e 87 radarspeedsetleft = 255;
joosthartkamp 20:9f4ba1b3d06e 88 radarspeedsetright = ((lidardirection-270)/90)*255;
joosthartkamp 20:9f4ba1b3d06e 89 Hbrug(radarspeedsetleft, radarspeedsetright, 1);
joosthartkamp 20:9f4ba1b3d06e 90 }
joosthartkamp 20:9f4ba1b3d06e 91 else if (lidardirection > 270) {
joosthartkamp 20:9f4ba1b3d06e 92 radarspeedsetleft = 255;
joosthartkamp 20:9f4ba1b3d06e 93 radarspeedsetright = ((lidardirection-270)/90)*255;
joosthartkamp 20:9f4ba1b3d06e 94 Hbrug(radarspeedsetleft, radarspeedsetright, 1);
joosthartkamp 20:9f4ba1b3d06e 95 }
joosthartkamp 20:9f4ba1b3d06e 96 }
joosthartkamp 20:9f4ba1b3d06e 97 }
joosthartkamp 20:9f4ba1b3d06e 98 }
joosthartkamp 20:9f4ba1b3d06e 99
joosthartkamp 20:9f4ba1b3d06e 100 }
joosthartkamp 18:3a613ea8e28c 101 int mapcurrent(float input = 0.00,float inputmin = 0.00, float inputmax = 0.00,int outputmin = 0, int outputmax = 0)
joosthartkamp 16:a9382753e8d6 102 {
joosthartkamp 16:a9382753e8d6 103 return (input - inputmin) * (outputmax - outputmin) / (inputmax - inputmin) + outputmin;
joosthartkamp 16:a9382753e8d6 104 }
bjorntukkertje 10:18116d979d61 105 // poar neemn
bjorntukkertje 10:18116d979d61 106 // twee poar neemn
bjorntukkertje 10:18116d979d61 107 // twee tettn in n envelop
bjorntukkertje 19:25663276160d 108 //neuken tot de vellen er bij hangen
joosthartkamp 20:9f4ba1b3d06e 109 /*
joosthartkamp 20:9f4ba1b3d06e 110 ******* radar direction ********************************
joosthartkamp 20:9f4ba1b3d06e 111 deze functie bepaalt de hoek waarin een andere robot is gedetecteerd
joosthartkamp 20:9f4ba1b3d06e 112 deze functie geeft een waarde terug van 0 tot 360
joosthartkamp 20:9f4ba1b3d06e 113
joosthartkamp 20:9f4ba1b3d06e 114 */
joosthartkamp 20:9f4ba1b3d06e 115 int lidardirection()
joosthartkamp 20:9f4ba1b3d06e 116 {
joosthartkamp 20:9f4ba1b3d06e 117 bool objectfound;
joosthartkamp 20:9f4ba1b3d06e 118 int startangle;
joosthartkamp 20:9f4ba1b3d06e 119 int stopangle;
joosthartkamp 20:9f4ba1b3d06e 120 int outputangle;
joosthartkamp 20:9f4ba1b3d06e 121 if (afstandzoeker == 1&&objectfound == 0) {
joosthartkamp 20:9f4ba1b3d06e 122 objectfound = 1;
joosthartkamp 20:9f4ba1b3d06e 123 int startangle = pos;
joosthartkamp 20:9f4ba1b3d06e 124
joosthartkamp 20:9f4ba1b3d06e 125 }
joosthartkamp 20:9f4ba1b3d06e 126 if (afstandzoeker == 0&&objectfound == 1) {
joosthartkamp 20:9f4ba1b3d06e 127 objectfound = 0;
joosthartkamp 20:9f4ba1b3d06e 128 stopangle = pos;
joosthartkamp 20:9f4ba1b3d06e 129 outputangle = startangle + (stopangle-startangle/2)
joosthartkamp 20:9f4ba1b3d06e 130 }
joosthartkamp 20:9f4ba1b3d06e 131 return outputangle;
joosthartkamp 20:9f4ba1b3d06e 132 }
joosthartkamp 20:9f4ba1b3d06e 133
bjorntukkertje 19:25663276160d 134
joosthartkamp 16:a9382753e8d6 135 int stepper()
bjorntukkertje 10:18116d979d61 136 {
bjorntukkertje 10:18116d979d61 137 switch (stapmode) {
bjorntukkertje 10:18116d979d61 138 case 0:
bjorntukkertje 10:18116d979d61 139 steppera = 1;
bjorntukkertje 10:18116d979d61 140 stepperb = 0;
bjorntukkertje 10:18116d979d61 141 stepperc = 1;
bjorntukkertje 10:18116d979d61 142 stepperd = 0;
bjorntukkertje 10:18116d979d61 143 pos ++;
bjorntukkertje 10:18116d979d61 144 break;
bjorntukkertje 10:18116d979d61 145 case 1:
bjorntukkertje 10:18116d979d61 146 steppera = 1;
bjorntukkertje 10:18116d979d61 147 stepperb = 0;
bjorntukkertje 10:18116d979d61 148 stepperc = 0;
bjorntukkertje 10:18116d979d61 149 stepperd = 1;
bjorntukkertje 10:18116d979d61 150 pos ++;
bjorntukkertje 10:18116d979d61 151 break;
bjorntukkertje 10:18116d979d61 152 case 2:
bjorntukkertje 10:18116d979d61 153 steppera = 0;
bjorntukkertje 10:18116d979d61 154 stepperb = 1;
bjorntukkertje 10:18116d979d61 155 stepperc = 0;
bjorntukkertje 10:18116d979d61 156 stepperd = 1;
bjorntukkertje 10:18116d979d61 157 pos ++;
bjorntukkertje 10:18116d979d61 158 break;
bjorntukkertje 10:18116d979d61 159 case 3:
bjorntukkertje 10:18116d979d61 160 steppera = 0;
bjorntukkertje 10:18116d979d61 161 stepperb = 1;
bjorntukkertje 10:18116d979d61 162 stepperc = 1;
bjorntukkertje 10:18116d979d61 163 stepperd = 0;
bjorntukkertje 10:18116d979d61 164 pos ++;
bjorntukkertje 10:18116d979d61 165 break;
bjorntukkertje 10:18116d979d61 166 ;
bjorntukkertje 10:18116d979d61 167 }
bjorntukkertje 10:18116d979d61 168 if (pos > 1000) { //na volledige rotatie ga naar nul
bjorntukkertje 13:02e56051501a 169 pos = 0;
bjorntukkertje 13:02e56051501a 170 }
joosthartkamp 16:a9382753e8d6 171 stapmode ++;
joosthartkamp 16:a9382753e8d6 172 if (stapmode > 3) {
joosthartkamp 16:a9382753e8d6 173 stapmode = 0;
joosthartkamp 16:a9382753e8d6 174 }
bjorntukkertje 10:18116d979d61 175 return pos;
bjorntukkertje 13:02e56051501a 176
bjorntukkertje 10:18116d979d61 177 }
bjorntukkertje 13:02e56051501a 178
joosthartkamp 18:3a613ea8e28c 179 bool afstandzoeker ()
bjorntukkertje 10:18116d979d61 180 {
joosthartkamp 18:3a613ea8e28c 181 bool a ;
joosthartkamp 18:3a613ea8e28c 182 if (zoeksensor > 0.25) {
joosthartkamp 18:3a613ea8e28c 183 a = 1;
joosthartkamp 18:3a613ea8e28c 184 } else {
joosthartkamp 18:3a613ea8e28c 185 a=0 ;
joosthartkamp 18:3a613ea8e28c 186 }
joosthartkamp 18:3a613ea8e28c 187 return a;
bjorntukkertje 10:18116d979d61 188 }
bjorntukkertje 13:02e56051501a 189
joosthartkamp 18:3a613ea8e28c 190 int lijnsensor ()
joosthartkamp 18:3a613ea8e28c 191
bjorntukkertje 10:18116d979d61 192 {
joosthartkamp 20:9f4ba1b3d06e 193 bool a = 0;// sensor linksvoor
joosthartkamp 20:9f4ba1b3d06e 194 bool b = 0;// sensor rechtsvoor
joosthartkamp 20:9f4ba1b3d06e 195 bool c = 0;// sensor linksachter
joosthartkamp 20:9f4ba1b3d06e 196 bool d = 0;// sensor rechtsachter
joosthartkamp 18:3a613ea8e28c 197
joosthartkamp 18:3a613ea8e28c 198 int output;
joosthartkamp 18:3a613ea8e28c 199 if (Sensor1 > 0.01) {
joosthartkamp 18:3a613ea8e28c 200 a = 1;
joosthartkamp 18:3a613ea8e28c 201 } else {
joosthartkamp 18:3a613ea8e28c 202 a = 0;
bjorntukkertje 10:18116d979d61 203 }
joosthartkamp 18:3a613ea8e28c 204 if (Sensor2 > 0.01) {
joosthartkamp 18:3a613ea8e28c 205 b = 1<<1;
joosthartkamp 18:3a613ea8e28c 206 } else {
joosthartkamp 18:3a613ea8e28c 207 b = 0<<1;
joosthartkamp 18:3a613ea8e28c 208 }
joosthartkamp 18:3a613ea8e28c 209 if (Sensor3 > 0.01) {
joosthartkamp 18:3a613ea8e28c 210 c = 1<<2;
joosthartkamp 18:3a613ea8e28c 211 } else {
joosthartkamp 18:3a613ea8e28c 212 c = 0<<2;
joosthartkamp 18:3a613ea8e28c 213 }
joosthartkamp 18:3a613ea8e28c 214 if (Sensor4 > 0.01) {
joosthartkamp 18:3a613ea8e28c 215 d = 1<<3;
joosthartkamp 18:3a613ea8e28c 216 } else {
joosthartkamp 18:3a613ea8e28c 217 d = 0<<3;
joosthartkamp 18:3a613ea8e28c 218 }
joosthartkamp 18:3a613ea8e28c 219
joosthartkamp 18:3a613ea8e28c 220 output = a || b || c || d;
joosthartkamp 18:3a613ea8e28c 221 return output;
bjorntukkertje 10:18116d979d61 222 }
bjorntukkertje 13:02e56051501a 223
bjorntukkertje 8:ec7b7c9ad07e 224
joosthartkamp 18:3a613ea8e28c 225
joosthartkamp 18:3a613ea8e28c 226 void Hbrug(int speedleft = 0, int speedright = 0, int direction = 0)
bjorntukkertje 13:02e56051501a 227 {
joosthartkamp 18:3a613ea8e28c 228 int currenta = mapcurrent(currentleftpin,0,1,0,6600);
joosthartkamp 18:3a613ea8e28c 229 int currentb = mapcurrent(currentrightpin,0,1,0,6600);
bjorntukkertje 13:02e56051501a 230
bjorntukkertje 13:02e56051501a 231
joosthartkamp 18:3a613ea8e28c 232 bool MotorL1 = 0;
joosthartkamp 18:3a613ea8e28c 233 bool MotorL2 = 0;
joosthartkamp 18:3a613ea8e28c 234 bool MotorR1 = 0;
joosthartkamp 18:3a613ea8e28c 235 bool MotorR2 = 0;
joosthartkamp 18:3a613ea8e28c 236
joosthartkamp 18:3a613ea8e28c 237 switch (stapmode) {
joosthartkamp 20:9f4ba1b3d06e 238 case 0:// stilstaan
joosthartkamp 18:3a613ea8e28c 239 MotorL1 = 0;
joosthartkamp 18:3a613ea8e28c 240 MotorL2 = 0;
joosthartkamp 18:3a613ea8e28c 241 MotorR1 = 0;
joosthartkamp 18:3a613ea8e28c 242 MotorR2 = 0;
joosthartkamp 18:3a613ea8e28c 243 break;
joosthartkamp 20:9f4ba1b3d06e 244 case 1:// vooruit
joosthartkamp 20:9f4ba1b3d06e 245 MotorL1 = 0;
joosthartkamp 20:9f4ba1b3d06e 246 MotorL2 = 1;
joosthartkamp 20:9f4ba1b3d06e 247 MotorR1 = 0;
joosthartkamp 20:9f4ba1b3d06e 248 MotorR2 = 1;
joosthartkamp 20:9f4ba1b3d06e 249 break;
joosthartkamp 20:9f4ba1b3d06e 250 case 2:// achteruit
joosthartkamp 18:3a613ea8e28c 251 MotorL1 = 0;
joosthartkamp 18:3a613ea8e28c 252 MotorL2 = 0;
joosthartkamp 18:3a613ea8e28c 253 MotorR1 = 0;
joosthartkamp 18:3a613ea8e28c 254 MotorR2 = 0;
joosthartkamp 18:3a613ea8e28c 255 break;
joosthartkamp 20:9f4ba1b3d06e 256 case 3:// naar linksvoor
joosthartkamp 18:3a613ea8e28c 257 MotorL1 = 0;
joosthartkamp 18:3a613ea8e28c 258 MotorL2 = 0;
joosthartkamp 18:3a613ea8e28c 259 MotorR1 = 0;
joosthartkamp 18:3a613ea8e28c 260 MotorR2 = 0;
joosthartkamp 18:3a613ea8e28c 261 break;
joosthartkamp 20:9f4ba1b3d06e 262 case 4:// naar rechtsvoor
joosthartkamp 18:3a613ea8e28c 263 MotorL1 = 0;
joosthartkamp 18:3a613ea8e28c 264 MotorL2 = 0;
joosthartkamp 18:3a613ea8e28c 265 MotorR1 = 0;
joosthartkamp 18:3a613ea8e28c 266 MotorR2 = 0;
joosthartkamp 18:3a613ea8e28c 267 break;
joosthartkamp 20:9f4ba1b3d06e 268 case 5:// naar linksachter
joosthartkamp 18:3a613ea8e28c 269 MotorL1 = 0;
joosthartkamp 18:3a613ea8e28c 270 MotorL2 = 0;
joosthartkamp 18:3a613ea8e28c 271 MotorR1 = 0;
joosthartkamp 18:3a613ea8e28c 272 MotorR2 = 0;
joosthartkamp 18:3a613ea8e28c 273 break;
joosthartkamp 20:9f4ba1b3d06e 274 case 6: // naar rechtsachter
joosthartkamp 18:3a613ea8e28c 275 MotorL1 = 0;
joosthartkamp 18:3a613ea8e28c 276 MotorL2 = 0;
joosthartkamp 18:3a613ea8e28c 277 MotorR1 = 0;
joosthartkamp 18:3a613ea8e28c 278 MotorR2 = 0;
joosthartkamp 18:3a613ea8e28c 279 break;
joosthartkamp 20:9f4ba1b3d06e 280 case 7: // links om as
joosthartkamp 18:3a613ea8e28c 281 MotorL1 = 0;
joosthartkamp 18:3a613ea8e28c 282 MotorL2 = 0;
joosthartkamp 18:3a613ea8e28c 283 MotorR1 = 0;
joosthartkamp 18:3a613ea8e28c 284 MotorR2 = 0;
joosthartkamp 18:3a613ea8e28c 285 break;
joosthartkamp 20:9f4ba1b3d06e 286 case 8: // rechts om as
joosthartkamp 18:3a613ea8e28c 287 MotorL1 = 0;
joosthartkamp 18:3a613ea8e28c 288 MotorL2 = 0;
joosthartkamp 18:3a613ea8e28c 289 MotorR1 = 0;
joosthartkamp 18:3a613ea8e28c 290 MotorR2 = 0;
joosthartkamp 18:3a613ea8e28c 291 break;
joosthartkamp 18:3a613ea8e28c 292
bjorntukkertje 13:02e56051501a 293 }
joosthartkamp 18:3a613ea8e28c 294 MotorL1pin = MotorL1;
joosthartkamp 18:3a613ea8e28c 295 MotorL2pin = MotorL2;
joosthartkamp 18:3a613ea8e28c 296 MotorR1pin = MotorR1;
joosthartkamp 18:3a613ea8e28c 297 MotorR2pin = MotorR2;
joosthartkamp 18:3a613ea8e28c 298
joosthartkamp 20:9f4ba1b3d06e 299 }