jordy morsinkhof / Mbed 2 deprecated Totaleprogramma

Mobiel platform besturen aan de hand van sensoren en een xbox controller

Dependencies:   USBHost USBHostXpad mbed-rtos mbed

main.cpp@0:345f76c72b9a, 2015-02-25 (annotated)

Committer:
347467
Date:
Wed Feb 25 08:25:16 2015 +0000
Revision:
0:345f76c72b9a
Nog niet kloar;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
347467 0:345f76c72b9a 1 #define DEBUG 0
347467 0:345f76c72b9a 2 #include "mbed.h"
347467 0:345f76c72b9a 3 #include "rtos.h"
347467 0:345f76c72b9a 4 #include "Traxster.h"
347467 0:345f76c72b9a 5 #include "USBHostXpad.h"
347467 0:345f76c72b9a 6 #include "utils.h"
347467 0:345f76c72b9a 7 #include "Audio.h"
347467 0:345f76c72b9a 8 #include <cmath>
347467 0:345f76c72b9a 9
347467 0:345f76c72b9a 10
347467 0:345f76c72b9a 11 Serial pc(USBTX, USBRX);
347467 0:345f76c72b9a 12 I2C i2cMod(p28, p27);
347467 0:345f76c72b9a 13
347467 0:345f76c72b9a 14 Serial robotCom(p13, p14);
347467 0:345f76c72b9a 15 AnalogIn ir(p20);
347467 0:345f76c72b9a 16 Speaker speaker(p18); // the pin must be the AnalogOut pin - p18
347467 0:345f76c72b9a 17
347467 0:345f76c72b9a 18 DigitalIn DI1(p5);
347467 0:345f76c72b9a 19 DigitalIn DI2(p6);
347467 0:345f76c72b9a 20 DigitalIn DI3(p7);
347467 0:345f76c72b9a 21 DigitalIn DI4(p8);
347467 0:345f76c72b9a 22 DigitalOut DQ1(p9); //motor lv
347467 0:345f76c72b9a 23 PwmOut PWM1(p21); //motor lv
347467 0:345f76c72b9a 24 DigitalOut DQ2(p10); //MOTOR rv
347467 0:345f76c72b9a 25 PwmOut PWM2 (p22); //Motor rv
347467 0:345f76c72b9a 26 DigitalOut DQ3(p11); //motor la
347467 0:345f76c72b9a 27 PwmOut PWM3(p23); //motor la
347467 0:345f76c72b9a 28 DigitalOut DQ4(p12); //MOTOR ra
347467 0:345f76c72b9a 29 PwmOut PWM4 (p24); //Motor ra
347467 0:345f76c72b9a 30 AnalogIn pot(p19); //potmeter
347467 0:345f76c72b9a 31 //I2C U1(p27,p28); //Ultrasoonsensorwaarde lv1
347467 0:345f76c72b9a 32 //I2C U2(p27,p28); //Ultrasoonsensorwaarde rv1
347467 0:345f76c72b9a 33 AnalogIn lslv(p15); //lasersensorwaarde lv2
347467 0:345f76c72b9a 34 AnalogIn lsrv(p16); //lasersensorwaarde rv2
347467 0:345f76c72b9a 35 DigitalOut led1(LED1); //shows trigger
347467 0:345f76c72b9a 36 DigitalOut led2(LED2); //shows collision prevention
347467 0:345f76c72b9a 37 DigitalOut led3(LED3); //shows laser pointer active
347467 0:345f76c72b9a 38 DigitalOut led4(LED4); //shows cpu activity
347467 0:345f76c72b9a 39 DigitalOut fire(p8);
347467 0:345f76c72b9a 40 DigitalOut laser(p27);
347467 0:345f76c72b9a 41 Audio audio (speaker);
347467 0:345f76c72b9a 42
347467 0:345f76c72b9a 43 Xbox360ControllerState controlState;
347467 0:345f76c72b9a 44 Traxster tank(robotCom);
347467 0:345f76c72b9a 45
347467 0:345f76c72b9a 46 int i2cAddress1 = 0xF2;
347467 0:345f76c72b9a 47 int i2cAddress2 = 0xE0;
347467 0:345f76c72b9a 48
347467 0:345f76c72b9a 49 void sendStartRangingCommand1(void){
347467 0:345f76c72b9a 50 const char command[] = {0x00, 0x51};
347467 0:345f76c72b9a 51 i2cMod.write(i2cAddress1, command, 2);
347467 0:345f76c72b9a 52 }
347467 0:345f76c72b9a 53 void sendStartRangingCommand2(void){
347467 0:345f76c72b9a 54 const char command[] = {0x00, 0x51};
347467 0:345f76c72b9a 55 i2cMod.write(i2cAddress2, command, 2);
347467 0:345f76c72b9a 56 }
347467 0:345f76c72b9a 57 int readRange1(void){
347467 0:345f76c72b9a 58 const char command[] = {0x02}; //Address of range register
347467 0:345f76c72b9a 59 char response[] = {0x00, 0x00};
347467 0:345f76c72b9a 60 i2cMod.write(i2cAddress1, command, 1, 1); //Send command
347467 0:345f76c72b9a 61 i2cMod.read(i2cAddress1, response, 2); //Read 16bits result
347467 0:345f76c72b9a 62 int range = (response[0]<<8)+response[1]; //Shift two bytes into int
347467 0:345f76c72b9a 63 return range;
347467 0:345f76c72b9a 64 }
347467 0:345f76c72b9a 65
347467 0:345f76c72b9a 66 int readRange2(void){
347467 0:345f76c72b9a 67 const char command[] = {0x02}; //Address of range register
347467 0:345f76c72b9a 68 char response[] = {0x00, 0x00};
347467 0:345f76c72b9a 69 i2cMod.write(i2cAddress2, command, 1, 1); //Send command
347467 0:345f76c72b9a 70 i2cMod.read(i2cAddress2, response, 2); //Read 16bits result
347467 0:345f76c72b9a 71 int range = (response[0]<<8)+response[1]; //Shift two bytes into int
347467 0:345f76c72b9a 72 return range;
347467 0:345f76c72b9a 73 }
347467 0:345f76c72b9a 74 void onControlInput(int buttons, int stick_lx, int stick_ly, int stick_rx, int stick_ry, int trigger_l, int trigger_r){
347467 0:345f76c72b9a 75 //pc.printf("buttons: %04x Lstick X,Y: %-5d, %-5d Rstick X,Y: %-5d, %-5d LTrig: %02x (%d) RTrig: %02x (%d)\r\n", buttons, stick_lx, stick_ly, stick_rx, stick_ry, trigger_l,trigger_l, trigger_r,trigger_r);
347467 0:345f76c72b9a 76 controlState = Xbox360ControllerState(buttons, stick_lx, stick_ly, stick_rx, stick_ry, trigger_l, trigger_r);
347467 0:345f76c72b9a 77 };
347467 0:345f76c72b9a 78
347467 0:345f76c72b9a 79 void thread_audio_run(void const* arg){
347467 0:345f76c72b9a 80 audio.run();
347467 0:345f76c72b9a 81 }
347467 0:345f76c72b9a 82
347467 0:345f76c72b9a 83 float tank_L(float x, float y){
347467 0:345f76c72b9a 84 float scale = 0.0;
347467 0:345f76c72b9a 85
347467 0:345f76c72b9a 86
347467 0:345f76c72b9a 87
347467 0:345f76c72b9a 88 if(x >= 0.0 && y <= 0){ //LBA bottom right
347467 0:345f76c72b9a 89 pc.printf("1\r\n");
347467 0:345f76c72b9a 90
347467 0:345f76c72b9a 91
347467 0:345f76c72b9a 92
347467 0:345f76c72b9a 93
347467 0:345f76c72b9a 94 if(y == 0){ scale = 1.0; goto End;}
347467 0:345f76c72b9a 95 if(x == 0){ scale = -1.0; goto End;}
347467 0:345f76c72b9a 96 float theta = atan(y/x) / 3.14159;
347467 0:345f76c72b9a 97 scale = theta * 4.0 + 1.0;
347467 0:345f76c72b9a 98
347467 0:345f76c72b9a 99
347467 0:345f76c72b9a 100 } else if(x <= 0.0 && y <= 0){ //LBA bottom left
347467 0:345f76c72b9a 101 pc.printf("2\r\n");
347467 0:345f76c72b9a 102
347467 0:345f76c72b9a 103
347467 0:345f76c72b9a 104
347467 0:345f76c72b9a 105 scale = -1.0;
347467 0:345f76c72b9a 106
347467 0:345f76c72b9a 107 } else if(x <= 0.0 && y >= 0){ //top left
347467 0:345f76c72b9a 108
347467 0:345f76c72b9a 109
347467 0:345f76c72b9a 110 if(y == 0){ scale = -1.0; goto End;}
347467 0:345f76c72b9a 111 if(x == 0){ scale = 1.0; goto End;}
347467 0:345f76c72b9a 112 float theta = atan(y/x) / 3.14159;
347467 0:345f76c72b9a 113 scale = -theta * 4.0 - 1.0;
347467 0:345f76c72b9a 114
347467 0:345f76c72b9a 115 } else { //top-right
347467 0:345f76c72b9a 116
347467 0:345f76c72b9a 117
347467 0:345f76c72b9a 118 scale = 1.0;
347467 0:345f76c72b9a 119 }
347467 0:345f76c72b9a 120
347467 0:345f76c72b9a 121 End:
347467 0:345f76c72b9a 122 scale *= sqrt(x*x + y*y);
347467 0:345f76c72b9a 123 return scale;
347467 0:345f76c72b9a 124 }
347467 0:345f76c72b9a 125 float tank_R(float x, float y){
347467 0:345f76c72b9a 126 float scale = 0.0;
347467 0:345f76c72b9a 127
347467 0:345f76c72b9a 128 if(x >= 0.0 && y <= 0){ //bottom right
347467 0:345f76c72b9a 129
347467 0:345f76c72b9a 130 scale = -1.0;
347467 0:345f76c72b9a 131
347467 0:345f76c72b9a 132 } else if(x <= 0.0 && y <= 0){ //bottom left
347467 0:345f76c72b9a 133 if(y == 0){ scale = 1.0; goto End;}
347467 0:345f76c72b9a 134 if(x == 0){ scale = -1.0; goto End;}
347467 0:345f76c72b9a 135 float theta = atan(y/x) / 3.14159;
347467 0:345f76c72b9a 136 scale = -theta*4.0 + 1.0;
347467 0:345f76c72b9a 137
347467 0:345f76c72b9a 138 } else if(x <= 0.0 && y >= 0){ //top left
347467 0:345f76c72b9a 139 scale = 1.0;
347467 0:345f76c72b9a 140
347467 0:345f76c72b9a 141 } else { //top-right
347467 0:345f76c72b9a 142 if(y == 0){ scale = -1.0; goto End;}
347467 0:345f76c72b9a 143 if(x == 0){ scale = 1.0; goto End;}
347467 0:345f76c72b9a 144 float theta = atan(y/x) / 3.14159;
347467 0:345f76c72b9a 145 scale = theta*4.0 - 1.0;
347467 0:345f76c72b9a 146 }
347467 0:345f76c72b9a 147
347467 0:345f76c72b9a 148 End:
347467 0:345f76c72b9a 149 scale *= sqrt(x*x + y*y);
347467 0:345f76c72b9a 150 return scale;
347467 0:345f76c72b9a 151 }
347467 0:345f76c72b9a 152 void thread_controller(void const* arg){
347467 0:345f76c72b9a 153
347467 0:345f76c72b9a 154 tank.SetMotors(0,0);
347467 0:345f76c72b9a 155 USBHostXpad controller;
347467 0:345f76c72b9a 156 controller.attachEvent(&onControlInput);
347467 0:345f76c72b9a 157
347467 0:345f76c72b9a 158 while(1){
347467 0:345f76c72b9a 159 tank.SetMotors(0,0);
347467 0:345f76c72b9a 160 bool wasdisconnected = true;
347467 0:345f76c72b9a 161 //acts as a failsafe
347467 0:345f76c72b9a 162 while(controller.connected()) {
347467 0:345f76c72b9a 163 if(wasdisconnected){
347467 0:345f76c72b9a 164 //pc.printf("Controller Status >> Connected!\r\n");
347467 0:345f76c72b9a 165 controller.led( USBHostXpad::LED1_ON );
347467 0:345f76c72b9a 166 tank.SetMotors(0.0,0.0); //stop, wait for controller to reconnect for a second.
347467 0:345f76c72b9a 167 wasdisconnected = false;
347467 0:345f76c72b9a 168 }
347467 0:345f76c72b9a 169
347467 0:345f76c72b9a 170 // left bumper disables auto-reverse
347467 0:345f76c72b9a 171 bool collisionAvoidance = !((bool)(controlState.buttons & USBHostXpad::XPAD_PAD_LB));
347467 0:345f76c72b9a 172 bool trigger = false;
347467 0:345f76c72b9a 173
347467 0:345f76c72b9a 174 // trigger rely - airsoft gun
347467 0:345f76c72b9a 175
347467 0:345f76c72b9a 176 float AS = 1.0; // acutele snelheid
347467 0:345f76c72b9a 177 if(controlState.triggerRight > 0x80){ //Rechtdoor rijden
347467 0:345f76c72b9a 178 //controller.rumble(255,255);
347467 0:345f76c72b9a 179 fire = 1;
347467 0:345f76c72b9a 180 trigger = true;
347467 0:345f76c72b9a 181
347467 0:345f76c72b9a 182
347467 0:345f76c72b9a 183 }else{
347467 0:345f76c72b9a 184 //controller.rumble(0,0);
347467 0:345f76c72b9a 185 fire = 0;
347467 0:345f76c72b9a 186 }
347467 0:345f76c72b9a 187
347467 0:345f76c72b9a 188 //lazzzzeeer sight
347467 0:345f76c72b9a 189 if( controlState.buttons & USBHostXpad::XPAD_PAD_X){ //Knop X laser=DQ3
347467 0:345f76c72b9a 190 //Stoppen
347467 0:345f76c72b9a 191 DQ1= 1; //lv
347467 0:345f76c72b9a 192 DQ2= 1; //rv
347467 0:345f76c72b9a 193 DQ3= 1; //la
347467 0:345f76c72b9a 194 DQ4= 1; //ra
347467 0:345f76c72b9a 195 PWM1 = AS*0; //lv in de vrij
347467 0:345f76c72b9a 196 PWM2 = AS*0; //rv in de vrij
347467 0:345f76c72b9a 197 PWM3 = AS*0; //la in de vrij
347467 0:345f76c72b9a 198 PWM4 = AS*0; //ra in de vrij
347467 0:345f76c72b9a 199 wait(2.0);
347467 0:345f76c72b9a 200
347467 0:345f76c72b9a 201 laser = 1;
347467 0:345f76c72b9a 202 }else{
347467 0:345f76c72b9a 203 laser = 0;
347467 0:345f76c72b9a 204 }
347467 0:345f76c72b9a 205
347467 0:345f76c72b9a 206
347467 0:345f76c72b9a 207 //dpad sounds
347467 0:345f76c72b9a 208 if(trigger || (controlState.buttons & (USBHostXpad::XPAD_HAT_UP | USBHostXpad::XPAD_HAT_DOWN | USBHostXpad::XPAD_HAT_LEFT | USBHostXpad::XPAD_HAT_RIGHT | USBHostXpad::XPAD_PAD_Y))){
347467 0:345f76c72b9a 209 int code = 0;
347467 0:345f76c72b9a 210
347467 0:345f76c72b9a 211 float z = 0.5; // persentage voor het draaien van de motoren
347467 0:345f76c72b9a 212 float j = 0.75; // persentage voor het draaien van de motoren
347467 0:345f76c72b9a 213 if(controlState.buttons & USBHostXpad::XPAD_HAT_UP){ //vooruit
347467 0:345f76c72b9a 214
347467 0:345f76c72b9a 215 DQ1= 1; //lv
347467 0:345f76c72b9a 216 DQ2= 1; //rv
347467 0:345f76c72b9a 217 DQ3= 1; //la
347467 0:345f76c72b9a 218 DQ4= 1; //ra
347467 0:345f76c72b9a 219 PWM1 = AS*1.5;
347467 0:345f76c72b9a 220 PWM2 = AS*1.5;
347467 0:345f76c72b9a 221 PWM3 = AS*1.5;
347467 0:345f76c72b9a 222 PWM4 = AS*1.5;
347467 0:345f76c72b9a 223 } else if(controlState.buttons & USBHostXpad::XPAD_HAT_DOWN){ //Achteruit
347467 0:345f76c72b9a 224 DQ1= 0; //lv
347467 0:345f76c72b9a 225 DQ2= 0; //rv
347467 0:345f76c72b9a 226 DQ3= 0; //la
347467 0:345f76c72b9a 227 DQ4= 0; //ra
347467 0:345f76c72b9a 228 PWM1 = AS*1.5;
347467 0:345f76c72b9a 229 PWM2 = AS*1.5;
347467 0:345f76c72b9a 230 PWM3 = AS*1.5;
347467 0:345f76c72b9a 231 PWM4 = AS*1.5;
347467 0:345f76c72b9a 232
347467 0:345f76c72b9a 233 } else if(controlState.buttons & USBHostXpad::XPAD_HAT_LEFT){ //links
347467 0:345f76c72b9a 234 led1=1; //links
347467 0:345f76c72b9a 235 DQ1 = 0; //lv
347467 0:345f76c72b9a 236 DQ2 = 1; //rv
347467 0:345f76c72b9a 237 DQ3 = 0; //La
347467 0:345f76c72b9a 238 DQ4 = 1; //Ra
347467 0:345f76c72b9a 239 PWM1 = AS*z;
347467 0:345f76c72b9a 240 PWM2 = AS*j;
347467 0:345f76c72b9a 241 PWM3 = AS*z;
347467 0:345f76c72b9a 242 PWM4 = AS*j;
347467 0:345f76c72b9a 243 } else if(controlState.buttons & USBHostXpad::XPAD_HAT_RIGHT){//rechts
347467 0:345f76c72b9a 244 led3=1; //rechts
347467 0:345f76c72b9a 245
347467 0:345f76c72b9a 246 DQ1 = 1; //lv
347467 0:345f76c72b9a 247 DQ2 = 0; //rv
347467 0:345f76c72b9a 248 DQ3 = 1; //la
347467 0:345f76c72b9a 249 DQ4 = 0; //Ra
347467 0:345f76c72b9a 250 PWM1 = AS*j;
347467 0:345f76c72b9a 251 PWM2 = AS*z;
347467 0:345f76c72b9a 252 PWM3 = AS*j;
347467 0:345f76c72b9a 253 PWM4 = AS*z;
347467 0:345f76c72b9a 254
347467 0:345f76c72b9a 255 } else if (trigger){
347467 0:345f76c72b9a 256 code = 4;
347467 0:345f76c72b9a 257 }else if (controlState.buttons & USBHostXpad::XPAD_PAD_Y){
347467 0:345f76c72b9a 258 code = 5;
347467 0:345f76c72b9a 259 }
347467 0:345f76c72b9a 260
347467 0:345f76c72b9a 261 //pc.printf("Audio play: %d\r\n",code);
347467 0:345f76c72b9a 262 audio.play(code);
347467 0:345f76c72b9a 263 }else{
347467 0:345f76c72b9a 264 //pc.printf("Audio Stop!\r\n");
347467 0:345f76c72b9a 265 audio.stop();
347467 0:345f76c72b9a 266 }
347467 0:345f76c72b9a 267
347467 0:345f76c72b9a 268
347467 0:345f76c72b9a 269
347467 0:345f76c72b9a 270 //update output leds
347467 0:345f76c72b9a 271 led1= fire;
347467 0:345f76c72b9a 272 led2= collisionAvoidance;
347467 0:345f76c72b9a 273 led3= laser;
347467 0:345f76c72b9a 274
347467 0:345f76c72b9a 275 // on collision, reverse tank
347467 0:345f76c72b9a 276 if(ir > 0.75 && collisionAvoidance){
347467 0:345f76c72b9a 277 // controller.rumble(255,255);
347467 0:345f76c72b9a 278 tank.SetMotors(-0.5,-0.5);
347467 0:345f76c72b9a 279 continue;
347467 0:345f76c72b9a 280 }else{
347467 0:345f76c72b9a 281 }
347467 0:345f76c72b9a 282
347467 0:345f76c72b9a 283 // map joystick input to tracks - if joystick is being moved
347467 0:345f76c72b9a 284 float y = xpadNormalizeAnalog(controlState.analogLeftY);
347467 0:345f76c72b9a 285 float x = -1.0 * xpadNormalizeAnalog(controlState.analogLeftX);
347467 0:345f76c72b9a 286 //float x = -1.0 * y / abs(y) * xpadNormalizeAnalog(controlState.analogLeftX); //flips left/right when in reverse
347467 0:345f76c72b9a 287 if( sqrt(x*x + y*y) > 0.25 ) {
347467 0:345f76c72b9a 288 tank.SetMotors(tank_L(x,y), tank_R(x,y));
347467 0:345f76c72b9a 289 //pc.printf("motors: %f, %f\r\n", tank_L(x,y), tank_R(x,y));
347467 0:345f76c72b9a 290 } else {
347467 0:345f76c72b9a 291 tank.SetMotors(0.0,0.0);
347467 0:345f76c72b9a 292 }
347467 0:345f76c72b9a 293
347467 0:345f76c72b9a 294
347467 0:345f76c72b9a 295
347467 0:345f76c72b9a 296 }
347467 0:345f76c72b9a 297
347467 0:345f76c72b9a 298 //pc.printf("Controller Status >> DISCONNECTED! Reconnecting...\r\n");
347467 0:345f76c72b9a 299 tank.SetMotors(0.0,0.0); //stop, wait for controller to reconnect for a second.
347467 0:345f76c72b9a 300 Thread::wait(500);
347467 0:345f76c72b9a 301 controller.connect();
347467 0:345f76c72b9a 302 }
347467 0:345f76c72b9a 303 }
347467 0:345f76c72b9a 304 int main() {
347467 0:345f76c72b9a 305
347467 0:345f76c72b9a 306 while(1) {
347467 0:345f76c72b9a 307 sendStartRangingCommand1();
347467 0:345f76c72b9a 308 wait(0.07);
347467 0:345f76c72b9a 309 int U1 = readRange1();
347467 0:345f76c72b9a 310 wait (0.2);
347467 0:345f76c72b9a 311 sendStartRangingCommand2();
347467 0:345f76c72b9a 312 wait(0.07);
347467 0:345f76c72b9a 313 int U2 = readRange2();
347467 0:345f76c72b9a 314
347467 0:345f76c72b9a 315
347467 0:345f76c72b9a 316
347467 0:345f76c72b9a 317 if (DI2 == 1 && DI4 == 1){
347467 0:345f76c72b9a 318 // sensoren
347467 0:345f76c72b9a 319
347467 0:345f76c72b9a 320 float AS; // snelheid afhangelijk van
347467 0:345f76c72b9a 321 float Q; // De afstand vanaf waar de ultrasonesensor iets moet detecteren in CM.
347467 0:345f76c72b9a 322 float I; // de afstand vanaf waar de infraroodsensor iets moet detecteren.
347467 0:345f76c72b9a 323 float x; // persentage voor het draaien van de motoren
347467 0:345f76c72b9a 324 float y; // persentage voor het draaien van de motoren
347467 0:345f76c72b9a 325 x = 0.5;
347467 0:345f76c72b9a 326 y = 0.25;
347467 0:345f76c72b9a 327 Q = 300; // groter dan een waarde 300 = 3 meter detecteerd de sensor iets
347467 0:345f76c72b9a 328 I = 1.5; // groeten dan een waarde 300 = 3meter
347467 0:345f76c72b9a 329 AS =pot*3.3;
347467 0:345f76c72b9a 330
347467 0:345f76c72b9a 331 if (U1<=Q && U2<=Q && lslv>=I && lsrv>=I)//alles iets detecteerd
347467 0:345f76c72b9a 332 {
347467 0:345f76c72b9a 333 DQ1= 1; //lv
347467 0:345f76c72b9a 334 DQ2= 1; //rv
347467 0:345f76c72b9a 335 DQ3= 1; //la
347467 0:345f76c72b9a 336 DQ4= 1; //ra
347467 0:345f76c72b9a 337 PWM1 = AS*0; //lv in de vrij
347467 0:345f76c72b9a 338 PWM2 = AS*0; //rv in de vrij
347467 0:345f76c72b9a 339 PWM3 = AS*0; //la in de vrij
347467 0:345f76c72b9a 340 PWM4 = AS*0; //ra in de vrij
347467 0:345f76c72b9a 341 wait(2.0);
347467 0:345f76c72b9a 342
347467 0:345f76c72b9a 343 while(1)
347467 0:345f76c72b9a 344 {
347467 0:345f76c72b9a 345 DQ1= 1; //Lv
347467 0:345f76c72b9a 346 DQ2= 0; //Rv
347467 0:345f76c72b9a 347 DQ3= 1; //La
347467 0:345f76c72b9a 348 DQ4= 0; //Ra
347467 0:345f76c72b9a 349 PWM1 = AS*0.5; //LV 50% van AS
347467 0:345f76c72b9a 350 PWM2 = AS*0.5; //RV 50% van AS
347467 0:345f76c72b9a 351 PWM3 = AS*0.5; //LA 50% van AS
347467 0:345f76c72b9a 352 PWM4 = AS*0.5; //RA 50% van AS
347467 0:345f76c72b9a 353
347467 0:345f76c72b9a 354 if(U1 >= Q && U2 >= Q)
347467 0:345f76c72b9a 355 {
347467 0:345f76c72b9a 356 break;
347467 0:345f76c72b9a 357 }
347467 0:345f76c72b9a 358
347467 0:345f76c72b9a 359 }
347467 0:345f76c72b9a 360 }
347467 0:345f76c72b9a 361
347467 0:345f76c72b9a 362
347467 0:345f76c72b9a 363 else if (U1<=Q && U2<=Q && lslv<=I && lsrv>=I)//rv1 rv2 en lv1 iets gedetecteerd hebben, naar links draaien
347467 0:345f76c72b9a 364 {
347467 0:345f76c72b9a 365 while(1)
347467 0:345f76c72b9a 366 {
347467 0:345f76c72b9a 367 DQ1 = 1; //lv
347467 0:345f76c72b9a 368 DQ2 = 1; //rv
347467 0:345f76c72b9a 369 DQ3 = 1; //La
347467 0:345f76c72b9a 370 DQ4 = 1; //Ra
347467 0:345f76c72b9a 371 PWM1 = AS*x;
347467 0:345f76c72b9a 372 PWM2 = AS*y;
347467 0:345f76c72b9a 373 PWM3 = AS*x;
347467 0:345f76c72b9a 374 PWM4 = AS*y;
347467 0:345f76c72b9a 375
347467 0:345f76c72b9a 376 if(U1 >= Q && U2 >= Q)
347467 0:345f76c72b9a 377 {
347467 0:345f76c72b9a 378 break;}
347467 0:345f76c72b9a 379 }
347467 0:345f76c72b9a 380 }
347467 0:345f76c72b9a 381
347467 0:345f76c72b9a 382 else if (U1<=Q && U2<=Q && lslv>=I && lsrv<=I)//lv1 lv2 en rv1 iets gedetecteerd hebben, naar rechts draaien)
347467 0:345f76c72b9a 383 {
347467 0:345f76c72b9a 384 while(1)
347467 0:345f76c72b9a 385 {
347467 0:345f76c72b9a 386 DQ1 = 1; //lv
347467 0:345f76c72b9a 387 DQ2 = 1; //rv
347467 0:345f76c72b9a 388 DQ3 = 1; //la
347467 0:345f76c72b9a 389 DQ4 = 1; //Ra
347467 0:345f76c72b9a 390 PWM1 = AS*y;
347467 0:345f76c72b9a 391 PWM2 = AS*x;
347467 0:345f76c72b9a 392 PWM3 = AS*y;
347467 0:345f76c72b9a 393 PWM4 = AS*x;
347467 0:345f76c72b9a 394
347467 0:345f76c72b9a 395
347467 0:345f76c72b9a 396 if(U1 >= Q && U2 >= Q)
347467 0:345f76c72b9a 397 {
347467 0:345f76c72b9a 398 break;}
347467 0:345f76c72b9a 399 }
347467 0:345f76c72b9a 400 }
347467 0:345f76c72b9a 401
347467 0:345f76c72b9a 402 else if (U1>Q && U2<Q && lslv<I && lsrv>I)//rv1 rv2 iets gedetecteerd hebben, naar links draaien)
347467 0:345f76c72b9a 403 {
347467 0:345f76c72b9a 404 while(1)
347467 0:345f76c72b9a 405 {
347467 0:345f76c72b9a 406 DQ1 = 1; //lv
347467 0:345f76c72b9a 407 DQ2 = 1; //rv
347467 0:345f76c72b9a 408 DQ3 = 1; //la
347467 0:345f76c72b9a 409 DQ4 = 1; //ra
347467 0:345f76c72b9a 410 PWM1 = AS*x;
347467 0:345f76c72b9a 411 PWM2 = AS*y;
347467 0:345f76c72b9a 412 PWM3 = AS*x;
347467 0:345f76c72b9a 413 PWM4 = AS*y;
347467 0:345f76c72b9a 414
347467 0:345f76c72b9a 415 if(U1>Q && U2 > Q)
347467 0:345f76c72b9a 416 {
347467 0:345f76c72b9a 417 break;}
347467 0:345f76c72b9a 418 }
347467 0:345f76c72b9a 419 }
347467 0:345f76c72b9a 420
347467 0:345f76c72b9a 421
347467 0:345f76c72b9a 422 else if (U1<Q && U2>Q && lslv>I && lsrv<I)//lv1 lv2 iets gedetecteerd hebben, naar rechts draaien)
347467 0:345f76c72b9a 423 {
347467 0:345f76c72b9a 424 while(1)
347467 0:345f76c72b9a 425 {
347467 0:345f76c72b9a 426 DQ1 = 1; //lv
347467 0:345f76c72b9a 427 DQ2 = 1; //rv
347467 0:345f76c72b9a 428 DQ3 = 1; //La
347467 0:345f76c72b9a 429 DQ4 = 1; //Ra
347467 0:345f76c72b9a 430 PWM1 = AS*y;
347467 0:345f76c72b9a 431 PWM2 = AS*x;
347467 0:345f76c72b9a 432 PWM3 = AS*y;
347467 0:345f76c72b9a 433 PWM4 = AS*x;
347467 0:345f76c72b9a 434
347467 0:345f76c72b9a 435 if(U1 > Q && U2 > Q)
347467 0:345f76c72b9a 436 {
347467 0:345f76c72b9a 437 break;}
347467 0:345f76c72b9a 438 }
347467 0:345f76c72b9a 439 }
347467 0:345f76c72b9a 440
347467 0:345f76c72b9a 441
347467 0:345f76c72b9a 442 else if (U1<Q && U2>Q && lslv<I && lsrv<I)//lv1 rv1 iets gedetecteerd hebben, naar links of rechts draaien)
347467 0:345f76c72b9a 443 {
347467 0:345f76c72b9a 444 if ( lslv>=lsrv)
347467 0:345f76c72b9a 445 {
347467 0:345f76c72b9a 446 while(1)
347467 0:345f76c72b9a 447 {
347467 0:345f76c72b9a 448 DQ1 = 1; //lv
347467 0:345f76c72b9a 449 DQ2 = 1; //rv
347467 0:345f76c72b9a 450 DQ3 = 1; //La
347467 0:345f76c72b9a 451 DQ4 = 1; //Ra
347467 0:345f76c72b9a 452 PWM1 = AS*y;
347467 0:345f76c72b9a 453 PWM2 = AS*x;
347467 0:345f76c72b9a 454 PWM3 = AS*y;
347467 0:345f76c72b9a 455 PWM4 = AS*x;
347467 0:345f76c72b9a 456
347467 0:345f76c72b9a 457 if(U1<Q && U2 > Q)
347467 0:345f76c72b9a 458 {
347467 0:345f76c72b9a 459 break;}
347467 0:345f76c72b9a 460 }
347467 0:345f76c72b9a 461 }
347467 0:345f76c72b9a 462
347467 0:345f76c72b9a 463 else if ( lslv<=lsrv )
347467 0:345f76c72b9a 464 {
347467 0:345f76c72b9a 465 while(1)
347467 0:345f76c72b9a 466 {
347467 0:345f76c72b9a 467 DQ1 = 1; //lv
347467 0:345f76c72b9a 468 DQ2 = 1; //rv
347467 0:345f76c72b9a 469 DQ3 = 1; //la
347467 0:345f76c72b9a 470 DQ4 = 1; //Ra
347467 0:345f76c72b9a 471 PWM1 = AS*x;
347467 0:345f76c72b9a 472 PWM2 = AS*y;
347467 0:345f76c72b9a 473 PWM3 = AS*x;
347467 0:345f76c72b9a 474 PWM4 = AS*y;
347467 0:345f76c72b9a 475
347467 0:345f76c72b9a 476 if( U1 > Q && U2 > Q)
347467 0:345f76c72b9a 477 {
347467 0:345f76c72b9a 478 break;}
347467 0:345f76c72b9a 479 }
347467 0:345f76c72b9a 480 }
347467 0:345f76c72b9a 481
347467 0:345f76c72b9a 482
347467 0:345f76c72b9a 483 else if (U1<Q && U2>Q && lslv<I && lsrv<I)//lv1 iets gedetecteerd heeft, naar rechts draaien)
347467 0:345f76c72b9a 484 {
347467 0:345f76c72b9a 485 while(1)
347467 0:345f76c72b9a 486 {
347467 0:345f76c72b9a 487
347467 0:345f76c72b9a 488 DQ1 = 1; //lv
347467 0:345f76c72b9a 489 DQ2 = 1; //rv
347467 0:345f76c72b9a 490 DQ3 = 1; //la
347467 0:345f76c72b9a 491 DQ4 = 1; //ra
347467 0:345f76c72b9a 492 PWM1 = AS*y;
347467 0:345f76c72b9a 493 PWM2 = AS*x;
347467 0:345f76c72b9a 494 PWM3 = AS*y;
347467 0:345f76c72b9a 495 PWM4 = AS*x;
347467 0:345f76c72b9a 496 if(U1>Q && U2 > Q)
347467 0:345f76c72b9a 497 {
347467 0:345f76c72b9a 498 break;}
347467 0:345f76c72b9a 499
347467 0:345f76c72b9a 500 }
347467 0:345f76c72b9a 501 }
347467 0:345f76c72b9a 502 //////////////////////////////////////////////////////////////////////////////
347467 0:345f76c72b9a 503 else if (DI1 == 1 && DI4 == 1)
347467 0:345f76c72b9a 504 {
347467 0:345f76c72b9a 505 // Handmatig
347467 0:345f76c72b9a 506 tank.SetMotors(0,0);
347467 0:345f76c72b9a 507 fire = 0;
347467 0:345f76c72b9a 508 laser = 1;
347467 0:345f76c72b9a 509 led1 = 1;
347467 0:345f76c72b9a 510 led2= 1;
347467 0:345f76c72b9a 511
347467 0:345f76c72b9a 512
347467 0:345f76c72b9a 513 //pc.baud(9600);
347467 0:345f76c72b9a 514 // pc.printf("TANK\r\n");
347467 0:345f76c72b9a 515
347467 0:345f76c72b9a 516
347467 0:345f76c72b9a 517
347467 0:345f76c72b9a 518 Thread t_controller(thread_controller);
347467 0:345f76c72b9a 519 Thread audio_thread(thread_audio_run);
347467 0:345f76c72b9a 520
347467 0:345f76c72b9a 521
347467 0:345f76c72b9a 522
347467 0:345f76c72b9a 523 tank.SetMotors(1.0,-1.0);
347467 0:345f76c72b9a 524 Thread::wait(1000);
347467 0:345f76c72b9a 525 tank.SetMotors(-1.0,1.0);
347467 0:345f76c72b9a 526 Thread::wait(1000);
347467 0:345f76c72b9a 527 tank.SetMotors(0,0);
347467 0:345f76c72b9a 528
347467 0:345f76c72b9a 529 while(1){
347467 0:345f76c72b9a 530 led4= !DQ4;
347467 0:345f76c72b9a 531 fire = !fire;
347467 0:345f76c72b9a 532 Thread::wait(1000);
347467 0:345f76c72b9a 533 }
347467 0:345f76c72b9a 534 }
347467 0:345f76c72b9a 535 }}}}
347467 0:345f76c72b9a 536
347467 0:345f76c72b9a 537
347467 0:345f76c72b9a 538
347467 0:345f76c72b9a 539
347467 0:345f76c72b9a 540