JJ Dekker
The project is not done yet
Dependencies: USBHost USBHostXpad mbed-rtos mbed
Fork of
Totaleprogramma
by 
jordy morsinkhof
Diff: main.cpp
- Revision:
- 0:345f76c72b9a
- Child:
- 1:da390b3b1330
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Feb 25 08:25:16 2015 +0000
@@ -0,0 +1,540 @@
+#define DEBUG 0
+#include "mbed.h"
+#include "rtos.h"
+#include "Traxster.h"
+#include "USBHostXpad.h"
+#include "utils.h"
+#include "Audio.h"
+#include <cmath>
+
+
+Serial pc(USBTX, USBRX);
+I2C i2cMod(p28, p27);
+
+Serial robotCom(p13, p14);
+AnalogIn ir(p20);
+Speaker speaker(p18); // the pin must be the AnalogOut pin - p18
+
+DigitalIn DI1(p5);
+DigitalIn DI2(p6);
+DigitalIn DI3(p7);
+DigitalIn DI4(p8);
+DigitalOut DQ1(p9); //motor lv
+PwmOut PWM1(p21); //motor lv
+DigitalOut DQ2(p10); //MOTOR rv
+PwmOut PWM2 (p22); //Motor rv
+DigitalOut DQ3(p11); //motor la
+PwmOut PWM3(p23); //motor la
+DigitalOut DQ4(p12); //MOTOR ra
+PwmOut PWM4 (p24); //Motor ra
+AnalogIn pot(p19); //potmeter
+//I2C U1(p27,p28); //Ultrasoonsensorwaarde lv1
+//I2C U2(p27,p28); //Ultrasoonsensorwaarde rv1
+AnalogIn lslv(p15); //lasersensorwaarde lv2
+AnalogIn lsrv(p16); //lasersensorwaarde rv2
+DigitalOut led1(LED1); //shows trigger
+DigitalOut led2(LED2); //shows collision prevention
+DigitalOut led3(LED3); //shows laser pointer active
+DigitalOut led4(LED4); //shows cpu activity
+DigitalOut fire(p8);
+DigitalOut laser(p27);
+Audio audio (speaker);
+
+Xbox360ControllerState controlState;
+Traxster tank(robotCom);
+
+int i2cAddress1 = 0xF2;
+int i2cAddress2 = 0xE0;
+
+void sendStartRangingCommand1(void){
+ const char command[] = {0x00, 0x51};
+ i2cMod.write(i2cAddress1, command, 2);
+}
+void sendStartRangingCommand2(void){
+ const char command[] = {0x00, 0x51};
+ i2cMod.write(i2cAddress2, command, 2);
+}
+int readRange1(void){
+ const char command[] = {0x02}; //Address of range register
+ char response[] = {0x00, 0x00};
+ i2cMod.write(i2cAddress1, command, 1, 1); //Send command
+ i2cMod.read(i2cAddress1, response, 2); //Read 16bits result
+ int range = (response[0]<<8)+response[1]; //Shift two bytes into int
+ return range;
+}
+
+int readRange2(void){
+ const char command[] = {0x02}; //Address of range register
+ char response[] = {0x00, 0x00};
+ i2cMod.write(i2cAddress2, command, 1, 1); //Send command
+ i2cMod.read(i2cAddress2, response, 2); //Read 16bits result
+ int range = (response[0]<<8)+response[1]; //Shift two bytes into int
+ return range;
+}
+void onControlInput(int buttons, int stick_lx, int stick_ly, int stick_rx, int stick_ry, int trigger_l, int trigger_r){
+ //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);
+ controlState = Xbox360ControllerState(buttons, stick_lx, stick_ly, stick_rx, stick_ry, trigger_l, trigger_r);
+};
+
+void thread_audio_run(void const* arg){
+ audio.run();
+}
+
+float tank_L(float x, float y){
+ float scale = 0.0;
+
+
+
+ if(x >= 0.0 && y <= 0){ //LBA bottom right
+ pc.printf("1\r\n");
+
+
+
+
+ if(y == 0){ scale = 1.0; goto End;}
+ if(x == 0){ scale = -1.0; goto End;}
+ float theta = atan(y/x) / 3.14159;
+ scale = theta * 4.0 + 1.0;
+
+
+ } else if(x <= 0.0 && y <= 0){ //LBA bottom left
+ pc.printf("2\r\n");
+
+
+
+ scale = -1.0;
+
+ } else if(x <= 0.0 && y >= 0){ //top left
+
+
+ if(y == 0){ scale = -1.0; goto End;}
+ if(x == 0){ scale = 1.0; goto End;}
+ float theta = atan(y/x) / 3.14159;
+ scale = -theta * 4.0 - 1.0;
+
+ } else { //top-right
+
+
+ scale = 1.0;
+ }
+
+ End:
+ scale *= sqrt(x*x + y*y);
+ return scale;
+}
+float tank_R(float x, float y){
+ float scale = 0.0;
+
+ if(x >= 0.0 && y <= 0){ //bottom right
+
+ scale = -1.0;
+
+ } else if(x <= 0.0 && y <= 0){ //bottom left
+ if(y == 0){ scale = 1.0; goto End;}
+ if(x == 0){ scale = -1.0; goto End;}
+ float theta = atan(y/x) / 3.14159;
+ scale = -theta*4.0 + 1.0;
+
+ } else if(x <= 0.0 && y >= 0){ //top left
+ scale = 1.0;
+
+ } else { //top-right
+ if(y == 0){ scale = -1.0; goto End;}
+ if(x == 0){ scale = 1.0; goto End;}
+ float theta = atan(y/x) / 3.14159;
+ scale = theta*4.0 - 1.0;
+ }
+
+ End:
+ scale *= sqrt(x*x + y*y);
+ return scale;
+}
+void thread_controller(void const* arg){
+
+ tank.SetMotors(0,0);
+ USBHostXpad controller;
+ controller.attachEvent(&onControlInput);
+
+ while(1){
+ tank.SetMotors(0,0);
+ bool wasdisconnected = true;
+ //acts as a failsafe
+ while(controller.connected()) {
+ if(wasdisconnected){
+ //pc.printf("Controller Status >> Connected!\r\n");
+ controller.led( USBHostXpad::LED1_ON );
+ tank.SetMotors(0.0,0.0); //stop, wait for controller to reconnect for a second.
+ wasdisconnected = false;
+ }
+
+ // left bumper disables auto-reverse
+ bool collisionAvoidance = !((bool)(controlState.buttons & USBHostXpad::XPAD_PAD_LB));
+ bool trigger = false;
+
+ // trigger rely - airsoft gun
+
+ float AS = 1.0; // acutele snelheid
+ if(controlState.triggerRight > 0x80){ //Rechtdoor rijden
+ //controller.rumble(255,255);
+ fire = 1;
+ trigger = true;
+
+
+ }else{
+ //controller.rumble(0,0);
+ fire = 0;
+ }
+
+ //lazzzzeeer sight
+ if( controlState.buttons & USBHostXpad::XPAD_PAD_X){ //Knop X laser=DQ3
+ //Stoppen
+ DQ1= 1; //lv
+ DQ2= 1; //rv
+ DQ3= 1; //la
+ DQ4= 1; //ra
+ PWM1 = AS*0; //lv in de vrij
+ PWM2 = AS*0; //rv in de vrij
+ PWM3 = AS*0; //la in de vrij
+ PWM4 = AS*0; //ra in de vrij
+ wait(2.0);
+
+ laser = 1;
+ }else{
+ laser = 0;
+ }
+
+
+ //dpad sounds
+ if(trigger || (controlState.buttons & (USBHostXpad::XPAD_HAT_UP | USBHostXpad::XPAD_HAT_DOWN | USBHostXpad::XPAD_HAT_LEFT | USBHostXpad::XPAD_HAT_RIGHT | USBHostXpad::XPAD_PAD_Y))){
+ int code = 0;
+
+ float z = 0.5; // persentage voor het draaien van de motoren
+ float j = 0.75; // persentage voor het draaien van de motoren
+ if(controlState.buttons & USBHostXpad::XPAD_HAT_UP){ //vooruit
+
+ DQ1= 1; //lv
+ DQ2= 1; //rv
+ DQ3= 1; //la
+ DQ4= 1; //ra
+ PWM1 = AS*1.5;
+ PWM2 = AS*1.5;
+ PWM3 = AS*1.5;
+ PWM4 = AS*1.5;
+ } else if(controlState.buttons & USBHostXpad::XPAD_HAT_DOWN){ //Achteruit
+ DQ1= 0; //lv
+ DQ2= 0; //rv
+ DQ3= 0; //la
+ DQ4= 0; //ra
+ PWM1 = AS*1.5;
+ PWM2 = AS*1.5;
+ PWM3 = AS*1.5;
+ PWM4 = AS*1.5;
+
+ } else if(controlState.buttons & USBHostXpad::XPAD_HAT_LEFT){ //links
+ led1=1; //links
+ DQ1 = 0; //lv
+ DQ2 = 1; //rv
+ DQ3 = 0; //La
+ DQ4 = 1; //Ra
+ PWM1 = AS*z;
+ PWM2 = AS*j;
+ PWM3 = AS*z;
+ PWM4 = AS*j;
+ } else if(controlState.buttons & USBHostXpad::XPAD_HAT_RIGHT){//rechts
+ led3=1; //rechts
+
+ DQ1 = 1; //lv
+ DQ2 = 0; //rv
+ DQ3 = 1; //la
+ DQ4 = 0; //Ra
+ PWM1 = AS*j;
+ PWM2 = AS*z;
+ PWM3 = AS*j;
+ PWM4 = AS*z;
+
+ } else if (trigger){
+ code = 4;
+ }else if (controlState.buttons & USBHostXpad::XPAD_PAD_Y){
+ code = 5;
+ }
+
+ //pc.printf("Audio play: %d\r\n",code);
+ audio.play(code);
+ }else{
+ //pc.printf("Audio Stop!\r\n");
+ audio.stop();
+ }
+
+
+
+ //update output leds
+ led1= fire;
+ led2= collisionAvoidance;
+ led3= laser;
+
+ // on collision, reverse tank
+ if(ir > 0.75 && collisionAvoidance){
+ // controller.rumble(255,255);
+ tank.SetMotors(-0.5,-0.5);
+ continue;
+ }else{
+ }
+
+ // map joystick input to tracks - if joystick is being moved
+ float y = xpadNormalizeAnalog(controlState.analogLeftY);
+ float x = -1.0 * xpadNormalizeAnalog(controlState.analogLeftX);
+ //float x = -1.0 * y / abs(y) * xpadNormalizeAnalog(controlState.analogLeftX); //flips left/right when in reverse
+ if( sqrt(x*x + y*y) > 0.25 ) {
+ tank.SetMotors(tank_L(x,y), tank_R(x,y));
+ //pc.printf("motors: %f, %f\r\n", tank_L(x,y), tank_R(x,y));
+ } else {
+ tank.SetMotors(0.0,0.0);
+ }
+
+
+
+ }
+
+ //pc.printf("Controller Status >> DISCONNECTED! Reconnecting...\r\n");
+ tank.SetMotors(0.0,0.0); //stop, wait for controller to reconnect for a second.
+ Thread::wait(500);
+ controller.connect();
+ }
+}
+int main() {
+
+ while(1) {
+ sendStartRangingCommand1();
+ wait(0.07);
+ int U1 = readRange1();
+ wait (0.2);
+ sendStartRangingCommand2();
+ wait(0.07);
+ int U2 = readRange2();
+
+
+
+ if (DI2 == 1 && DI4 == 1){
+ // sensoren
+
+ float AS; // snelheid afhangelijk van
+ float Q; // De afstand vanaf waar de ultrasonesensor iets moet detecteren in CM.
+ float I; // de afstand vanaf waar de infraroodsensor iets moet detecteren.
+ float x; // persentage voor het draaien van de motoren
+ float y; // persentage voor het draaien van de motoren
+ x = 0.5;
+ y = 0.25;
+ Q = 300; // groter dan een waarde 300 = 3 meter detecteerd de sensor iets
+ I = 1.5; // groeten dan een waarde 300 = 3meter
+ AS =pot*3.3;
+
+ if (U1<=Q && U2<=Q && lslv>=I && lsrv>=I)//alles iets detecteerd
+ {
+ DQ1= 1; //lv
+ DQ2= 1; //rv
+ DQ3= 1; //la
+ DQ4= 1; //ra
+ PWM1 = AS*0; //lv in de vrij
+ PWM2 = AS*0; //rv in de vrij
+ PWM3 = AS*0; //la in de vrij
+ PWM4 = AS*0; //ra in de vrij
+ wait(2.0);
+
+ while(1)
+ {
+ DQ1= 1; //Lv
+ DQ2= 0; //Rv
+ DQ3= 1; //La
+ DQ4= 0; //Ra
+ PWM1 = AS*0.5; //LV 50% van AS
+ PWM2 = AS*0.5; //RV 50% van AS
+ PWM3 = AS*0.5; //LA 50% van AS
+ PWM4 = AS*0.5; //RA 50% van AS
+
+ if(U1 >= Q && U2 >= Q)
+ {
+ break;
+ }
+
+ }
+ }
+
+
+ else if (U1<=Q && U2<=Q && lslv<=I && lsrv>=I)//rv1 rv2 en lv1 iets gedetecteerd hebben, naar links draaien
+ {
+ while(1)
+ {
+ DQ1 = 1; //lv
+ DQ2 = 1; //rv
+ DQ3 = 1; //La
+ DQ4 = 1; //Ra
+ PWM1 = AS*x;
+ PWM2 = AS*y;
+ PWM3 = AS*x;
+ PWM4 = AS*y;
+
+ if(U1 >= Q && U2 >= Q)
+ {
+ break;}
+ }
+ }
+
+ else if (U1<=Q && U2<=Q && lslv>=I && lsrv<=I)//lv1 lv2 en rv1 iets gedetecteerd hebben, naar rechts draaien)
+ {
+ while(1)
+ {
+ DQ1 = 1; //lv
+ DQ2 = 1; //rv
+ DQ3 = 1; //la
+ DQ4 = 1; //Ra
+ PWM1 = AS*y;
+ PWM2 = AS*x;
+ PWM3 = AS*y;
+ PWM4 = AS*x;
+
+
+ if(U1 >= Q && U2 >= Q)
+ {
+ break;}
+ }
+ }
+
+ else if (U1>Q && U2<Q && lslv<I && lsrv>I)//rv1 rv2 iets gedetecteerd hebben, naar links draaien)
+ {
+ while(1)
+ {
+ DQ1 = 1; //lv
+ DQ2 = 1; //rv
+ DQ3 = 1; //la
+ DQ4 = 1; //ra
+ PWM1 = AS*x;
+ PWM2 = AS*y;
+ PWM3 = AS*x;
+ PWM4 = AS*y;
+
+ if(U1>Q && U2 > Q)
+ {
+ break;}
+ }
+ }
+
+
+ else if (U1<Q && U2>Q && lslv>I && lsrv<I)//lv1 lv2 iets gedetecteerd hebben, naar rechts draaien)
+ {
+ while(1)
+ {
+ DQ1 = 1; //lv
+ DQ2 = 1; //rv
+ DQ3 = 1; //La
+ DQ4 = 1; //Ra
+ PWM1 = AS*y;
+ PWM2 = AS*x;
+ PWM3 = AS*y;
+ PWM4 = AS*x;
+
+ if(U1 > Q && U2 > Q)
+ {
+ break;}
+ }
+ }
+
+
+ else if (U1<Q && U2>Q && lslv<I && lsrv<I)//lv1 rv1 iets gedetecteerd hebben, naar links of rechts draaien)
+ {
+ if ( lslv>=lsrv)
+ {
+ while(1)
+ {
+ DQ1 = 1; //lv
+ DQ2 = 1; //rv
+ DQ3 = 1; //La
+ DQ4 = 1; //Ra
+ PWM1 = AS*y;
+ PWM2 = AS*x;
+ PWM3 = AS*y;
+ PWM4 = AS*x;
+
+ if(U1<Q && U2 > Q)
+ {
+ break;}
+ }
+ }
+
+ else if ( lslv<=lsrv )
+ {
+ while(1)
+ {
+ DQ1 = 1; //lv
+ DQ2 = 1; //rv
+ DQ3 = 1; //la
+ DQ4 = 1; //Ra
+ PWM1 = AS*x;
+ PWM2 = AS*y;
+ PWM3 = AS*x;
+ PWM4 = AS*y;
+
+ if( U1 > Q && U2 > Q)
+ {
+ break;}
+ }
+ }
+
+
+ else if (U1<Q && U2>Q && lslv<I && lsrv<I)//lv1 iets gedetecteerd heeft, naar rechts draaien)
+ {
+ while(1)
+ {
+
+ DQ1 = 1; //lv
+ DQ2 = 1; //rv
+ DQ3 = 1; //la
+ DQ4 = 1; //ra
+ PWM1 = AS*y;
+ PWM2 = AS*x;
+ PWM3 = AS*y;
+ PWM4 = AS*x;
+ if(U1>Q && U2 > Q)
+ {
+ break;}
+
+ }
+ }
+//////////////////////////////////////////////////////////////////////////////
+ else if (DI1 == 1 && DI4 == 1)
+ {
+ // Handmatig
+ tank.SetMotors(0,0);
+ fire = 0;
+ laser = 1;
+ led1 = 1;
+ led2= 1;
+
+
+ //pc.baud(9600);
+ // pc.printf("TANK\r\n");
+
+
+
+ Thread t_controller(thread_controller);
+ Thread audio_thread(thread_audio_run);
+
+
+
+ tank.SetMotors(1.0,-1.0);
+ Thread::wait(1000);
+ tank.SetMotors(-1.0,1.0);
+ Thread::wait(1000);
+ tank.SetMotors(0,0);
+
+ while(1){
+ led4= !DQ4;
+ fire = !fire;
+ Thread::wait(1000);
+ }
+ }
+ }}}}
+
+
+
+
+
\ No newline at end of file