Diff: main.cpp

Revision:

1:248c00db9e9d

Parent:

0:27339bb50fd6

--- a/main.cpp	Tue Jul 13 01:51:12 2021 +0000
+++ b/main.cpp	Mon Jul 26 08:37:10 2021 +0000
@@ -1,31 +1,70 @@
 #include "mbed.h"
+#include "servo_ppm.h"
 
 #define CAN_TD D2
 #define CAN_RD D10
 
 #define INTERRUPT D8
 
-#define LEFTJOYSTICK true
+#define ARM_SWITCH D7
+#define MODE_SWITCH D6
+
+#define PPM_PIN D13
+
 
 I2C i2c(I2C_SDA, I2C_SCL);
 InterruptIn interrupt(INTERRUPT);
+
+//**FOR SERIAL OUTPUT USING PUTTY                 using 64-bit avaialble from https://www.chiark.greenend.org.uk/~sgtatham/putty/latest.html
 Serial pc(USBTX, USBRX);
 
 CAN can1(CAN_RD, CAN_TD);
 
-const int main_addr = 0x81;
-const int alt_addr = 0x83;
-const int control = 0x76;
+//PPM Output
+servo_ppm ppm(PPM_PIN);
+
+//Digital Inputs
+DigitalIn modeSwitch(MODE_SWITCH);
+DigitalIn armSwitch(ARM_SWITCH);
+
+const int left_addr = 0x81;
+const int right_addr = 0x83;
 
-char rawReading[2];
+//const int control = 0x76;
+
+char left_rawReading[2];
+char right_rawReading[2];
 
-int xAxis = 0;
-int yAxis = 0;
+int8_t left_xAxis = 0;
+int8_t left_yAxis = 0;
+int8_t right_xAxis = 0;
+int8_t right_yAxis = 0;
+int8_t arm_switch = 0;
+int8_t mode_switch = 0;
 
 int main() {
+    pc.baud(115200); 
+    pc.printf("Starting I2C converter \n\r");
+    //interrupt.fall(&readSensor);
     
-    pc.printf("Starting I2C converter \n");
-    //interrupt.fall(&readSensor);
+    //right stick
+    ppm.setServoPulseDuration_us(1, 1500);
+    ppm.setServoPulseDuration_us(2, 1500);
+    //left stick
+    ppm.setServoPulseDuration_us(3, 1500);
+    ppm.setServoPulseDuration_us(4, 1500);
+    //buttons   
+    ppm.setServoPulseDuration_us(5, 1000);
+    ppm.setServoPulseDuration_us(6, 1000);
+    //unused
+    ppm.setServoPulseDuration_us(7, 1500);
+    ppm.setServoPulseDuration_us(8, 1500);
+    
+    //start ppm
+    ppm.startServoPpmOutput();
+    
+    modeSwitch.mode(PullDown);
+    armSwitch.mode(PullDown);
     
     can1.frequency(250000);
     //can1.filter(0x70,0x70,CANExtended); //CANbus filtering to only accept 0x70
@@ -36,37 +75,61 @@
     msg.id = 0x71;   //messageID
     msg.len = 8;     //length in bytes
     
-    for(int i = 3; i<8; i++) {
+    for(int i = 4; i<8; i++) {
             msg.data[i] = 0;
     }
     
-    int addr = main_addr;
+    wait(1.0);
+
     
-    if(LEFTJOYSTICK) {
-        addr = main_addr;
-        msg.data[0] = 0;
-    }
-    else {
-        addr = alt_addr;
-        msg.data[0] = 1;
-    }
+    while(1) {
+        i2c.read(left_addr, left_rawReading, 2);
+        left_xAxis = (int8_t)(left_rawReading[0]);
+        left_yAxis = (int8_t)(left_rawReading[1]);
+        msg.data[0] = left_xAxis;
+        msg.data[1] = left_yAxis;
+        pc.printf("Left Raw Reading X: %c, Y: %c \n\r", left_rawReading[0], left_rawReading[1]);    
+        pc.printf("Left X: %d, Y: %d \n\r", left_xAxis, left_yAxis);
+        
+        i2c.read(right_addr, right_rawReading, 2);
+        right_xAxis = (int8_t)(right_rawReading[0]);
+        right_yAxis = (int8_t)(right_rawReading[1]);
+        msg.data[2] = right_xAxis;
+        msg.data[3] = right_yAxis;
+        pc.printf("Right Raw Reading X: %c, Y: %c \n\r", right_rawReading[0], right_rawReading[1]);        
+        pc.printf("Right X: %d, Y: %d \n\r", right_xAxis, right_yAxis);
         
-    while(1) {
+        arm_switch = armSwitch.read();
+        mode_switch = modeSwitch.read();
+        msg.data[4] = arm_switch;
+        msg.data[5] = mode_switch;
+        
+        pc.printf("Arm Switch: %d, Mode Switch: %d \n\r", arm_switch, mode_switch);
         
-        i2c.read(addr, rawReading, 2);
-        xAxis = (int8_t)(rawReading[0]);
-        yAxis = (int8_t)(rawReading[1]);
-        msg.data[1] = xAxis;
-        msg.data[2] = yAxis;
-
-        //pc.printf("X: %d, Y: %d \n", xAxis, yAxis);
+        //left Y axis - Throttle (Pulse 3)
+        ppm.setServoPulseDuration_us(3, (left_yAxis+80)*1000/160+1000);
+    
+        //left X axis - Yaw (Pulse 4)
+        ppm.setServoPulseDuration_us(4, (left_xAxis+80)*1000/160+1000);
+    
+        //right Y Axis - Pitch (Pulse 2)
+        ppm.setServoPulseDuration_us(2, (right_yAxis+80)*1000/160+1000);
+    
+        //right X Axis - Roll (Pulse 1)
+        ppm.setServoPulseDuration_us(1, (right_xAxis+80)*1000/160+1000);
+    
+        //modeSwitch - Pulse 6
+        ppm.setServoPulseDuration_us(6, mode_switch*1000+1000);
+    
+        //armSwitch - Pulse 5
+        ppm.setServoPulseDuration_us(5, arm_switch*1000+1000);
         
+        //send out CANBus value
         can1.write(msg);
         if(can1.read(msg)) {
-            //pc.printf("Recv: %d\n", msg.id);    
+            pc.printf("Recv: %d\n\r", msg.id);    
         }
         
-        //send out CANBus value
         wait(0.02);
     }
-}
+}
\ No newline at end of file