Diff: main.cpp

Revision:

3:c1620db50a75

Parent:

2:5e870c215495

Child:

4:868a07a5496f

--- a/main.cpp	Sat Nov 15 21:42:16 2014 +0000
+++ b/main.cpp	Wed Nov 26 22:53:15 2014 +0000
@@ -1,15 +1,24 @@
+
+#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);
+
+
+//Serial pc(USBTX, USBRX);
 Serial robotCom(p13, p14);
 AnalogIn ir(p20);
 //Speaker speaker(p18);  // the pin must be the AnalogOut pin - p18
 DigitalOut led(LED1);
+DigitalOut led4(LED4);
+
+Mutex m;
 
 
 //Audio audio(speaker);
@@ -19,7 +28,7 @@
 //comment added
 
 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);
+    //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);
 };
 
@@ -27,32 +36,102 @@
 //    audio.run();
 //}
 
+float tank_L(float x, float y){
+    float scale = 0.0;
+    
+    if(x >= 0.0 && y <= 0){  //bottom 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;
+        
+    } else if(x <= 0.0 && y <= 0){  //bottom left
+        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){
     
     USBHostXpad controller;
     controller.attachEvent(&onControlInput);
     
     while(1){
+        bool wasdisconnected = true;
         //acts as a failsafe
         while(controller.connected()) {
-            if(ir > 0.5){
-                controller.rumble(255,255);
+            if(wasdisconnected){
+                //pc.printf("Controller Status >> Connected!\r\n");
+                controller.led( USBHostXpad::LED1_ON );   
+                wasdisconnected = false;
             }
-            if( controller.read(USBHostXpad::XPAD_PAD_Y) ) {
-                controller.led( USBHostXpad::LED_ROTATE );   
-            } else {
-                controller.led( USBHostXpad::LED_OFF ); 
+            
+            //left joystick controls
+            float y = xpadNormalizeAnalog(controlState.analogLeftY);
+            float x = -1.0 * y / abs(y) *  xpadNormalizeAnalog(controlState.analogLeftX);
+            
+            if(ir > 0.75){      // on collision, stop tank
+                controller.rumble(255,255);
+                tank.SetMotors(0,0); 
+            }else{
             }
-            if(controlState.triggerRight > 0){
+            
+            if(controlState.triggerRight > 0x80){   //shoot
                 controller.rumble(255,255);
+                led = 1;
+            }else{
+                controller.rumble(0,0);
+                led = 0;
             }
            
-            if( controller.read(USBHostXpad::XPAD_PAD_X) ) {
-                tank.SetMotors(1.0,1.0);
+            if( sqrt(x*x + y*y) > 0.25 ) {
+                tank.SetMotors(tank_L(x,y), tank_R(x,y));
+                //m.lock();
+                //pc.printf("motors: %f, %f\r\n", tank_L(x,y), tank_R(x,y));
+                //m.unlock();
             } else {
-                tank.SetMotors(0.0,0.0);    //stop, wait for controller to reconnect for a second.   
+                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();
@@ -61,8 +140,10 @@
 
 int main() {
     led = 1;
-    pc.baud(19200);
-    pc.printf("TANK\r\n");
+    
+    led4 = 1;
+    //pc.baud(19200);
+    //pc.printf("TANK\r\n");
     
     Thread t_controller(thread_controller);
     
@@ -70,7 +151,16 @@
     
     //audio.playMario();
     
+    
+    //tank.SetMotors(1.0,-1.0);
+    //Thread::wait(1000);
+    //tank.SetMotors(-1.0,1.0);
+    //Thread::wait(1000);
+    //tank.SetMotors(0,0);
+    
     while(1){
-        Thread::wait(0);
+        led4 = !led4;
+        Thread::wait(500);
     }
 }
+