Diff: main.cpp

Revision:

1:346022f10041

Parent:

0:e43878690c0e

Child:

2:e9926793544d

--- a/main.cpp	Thu Jan 05 14:23:14 2017 +0000
+++ b/main.cpp	Fri Jan 13 07:11:30 2017 +0000
@@ -32,79 +32,98 @@
 USBJoystick joystick;
 
 // Variables for Heartbeat and Status monitoring
-DigitalOut myled1(LED1);
-DigitalOut myled2(LED2);
-DigitalOut myled3(LED3);
-DigitalOut heartbeatLED(LED4);
+PwmOut myled1(LED1);
+PwmOut myled2(LED2);
+PwmOut myled3(LED3);
+PwmOut heartbeatLED(LED4);
+
+AnalogIn joyXin(A0);
+AnalogIn joyYin(A1);
+AnalogIn joyZin(A2);
+AnalogIn joyRxin(A3);
+AnalogIn joyRyin(A4);
+AnalogIn joyRzin(A5);
 
 Ticker heartbeat;
 Serial pc(USBTX, USBRX); // tx, rx
 
-// Heartbeat monitor
-void pulse() {
-  heartbeatLED = !heartbeatLED;
+int16_t map(int32_t x, int32_t in_min, int32_t in_max, int32_t out_min, int32_t out_max) // found here: C:\Program Files (x86)\Arduino\hardware\arduino\avr\cores\arduino\WMath.cpp
+{
+  return (int16_t)((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min);
+}
+
+float mapf(float x, float in_min, float in_max, float out_min, float out_max) {
+    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
 
-void heartbeat_start() {
-  heartbeatLED=1;
-  heartbeat.attach(&pulse, 0.5);
-}
-
-void heartbeat_stop() {
-  heartbeat.detach();
-}
+// Heartbeat monitor
+//void pulse() {
+//  heartbeatLED = !heartbeatLED;
+//}
+//
+//void heartbeat_start() {
+//  heartbeatLED=1;
+//  heartbeat.attach(&pulse, 0.5);
+//}
+//
+//void heartbeat_stop() {
+//  heartbeat.detach();
+//}
 
 
 int main() {
   uint16_t i = 0;
-  int16_t throttle = 0;
-  int16_t rudder = 0;    
   int16_t x = 0;
   int16_t y = 0;
-  int32_t radius = 120;
-  int32_t angle = 0;
+  int16_t z = 0;
+  int16_t rx = 0;
+  int16_t ry = 0;
+  int16_t rz = 0;
   uint8_t tmp = 0;
-  uint32_t buttons = 0;    
-  uint8_t hat = 0;    
+  uint32_t buttons = 0;
+  uint8_t hat = 0;
     
   pc.printf("Hello World from Joystick!\n\r");
 
-  heartbeat_start();
+//  heartbeat_start();
 
   while (1) {
-    // Basic Joystick
-    throttle = (i >> 8) & 0xFF; // value -127 .. 128
-    rudder = (i >> 8) & 0xFF;   // value -127 .. 128        
+    // 6DOF Joystick with buttons and a hat
 
-#if (BUTTONS4 == 1)        
-    buttons = (i >> 8) & 0x0F;   // value    0 ..  15, one bit per button     
-#endif        
-#if (BUTTONS8 == 1)        
-    buttons = (i >> 8) & 0xFF;   // value    0 .. 255, one bit per button     
-#endif        
-#if (BUTTONS32 == 1)        
-    tmp     = (i >> 8) & 0xFF;   // value    0 .. 255, one bit per button     
+#if (BUTTONS4 == 1)
+    buttons = (i >> 8) & 0x0F;   // value    0 ..  15, one bit per button
+#endif
+#if (BUTTONS8 == 1)
+    buttons = (i >> 8) & 0xFF;   // value    0 .. 255, one bit per button
+#endif
+#if (BUTTONS32 == 1)
+    tmp     = (i >> 8) & 0xFF;   // value    0 .. 255, one bit per button
     buttons =           (( tmp <<  0) & 0x000000FF);
     buttons = buttons | ((~tmp <<  8) & 0x0000FF00);
     buttons = buttons | (( tmp << 16) & 0x00FF0000);
     buttons = buttons | ((~tmp << 24) & 0xFF000000);
-#endif        
+#endif
 
-#if (HAT4 == 1)        
-    hat    = (i >> 8) & 0x03;   // value 0, 1, 2, 3 or 4 for neutral 
+#if (HAT4 == 1)
+    hat    = (i >> 8) & 0x03;   // value 0, 1, 2, 3 or 4 for neutral
 #endif
 #if (HAT8 == 1)
     hat    = (i >> 8) & 0x07;   // value 0..7 or 8 for neutral
-#endif        
-    i++;        
-        
-    x = cos((double)angle*3.14/180.0)*radius;  // value -127 .. 128
-    y = sin((double)angle*3.14/180.0)*radius;  // value -127 .. 128
-    angle += 3;        
+#endif
+    i++;
+    
+//    joystick.move(joyXin, joyYin, joyZin, joyRxin, joyRyin, joyRzin);
 
-    joystick.update(throttle, rudder, x, y, buttons, hat);
-    wait(0.001);
+    x = map(joyXin.read_u16(), 0, 65535, -32768, 32768);  // value -32768 .. 32767
+    y = map(joyYin.read_u16(), 0, 65535, -32768, 32768);
+    z = map(joyZin.read_u16(), 0, 65535, -32768, 32768);
+    rx = map(joyRxin.read_u16(), 0, 65535, -32768, 32768);
+    ry = map(joyRyin.read_u16(), 0, 65535, -32768, 32768);
+    rz = map(joyRzin.read_u16(), 0, 65535, -32768, 32768);
+    
+    myled1 = mapf(x, -32768, 32768, 0, 1.0);
+        
+    joystick.update(x, y, z, rx, ry, rz, buttons, hat);
+    wait(0.005);
   }
-    
-  pc.printf("Bye World!\n\r");                           
 }
\ No newline at end of file