Diff: main.cpp

Revision:

1:b3151b6fe36b

Parent:

0:032a5658acdc

--- a/main.cpp	Tue Mar 16 04:00:51 2021 +0000
+++ b/main.cpp	Tue Apr 06 19:59:07 2021 +0000
@@ -1,19 +1,11 @@
-// Keep sweeping servo from left to right
 #include "mbed.h"
-#include "Servo.h"
-
-#define JOYSTICK_ID 0x0cfdd633 //can also be 0x0CFDD633
-#define CAN_TD D2
-#define CAN_RD D10    
+#include "servo_ppm.h"
 
-//PWM Outputs
-Servo leftESC(D3);
-Servo rightESC(A6);
-
-int leftDuty = 1500;
-int rightDuty = 1500;
-
-bool deadMan = false;
+#define JOYSTICK_ID_L 0x0cfdd633
+#define JOYSTICK_ID_R 0x0cfdd634
+#define CAN_TD D2
+#define CAN_RD D10
+#define PPM_PIN A6
 
 //CANBus                                               
 CAN can(CAN_RD, CAN_TD);
@@ -21,142 +13,128 @@
 //**FOR SERIAL OUTPUT USING PUTTY                 using 64-bit avaialble from https://www.chiark.greenend.org.uk/~sgtatham/putty/latest.html                                             
 Serial pc(SERIAL_TX, SERIAL_RX);  
 
-Ticker ticker;
+//PPM Output
+servo_ppm ppm(PPM_PIN);
     
     //Function to read the CANbus and decode the data
     void readCAN(void) {
         CANMessage msg;
         if(can.read(msg)) {
             
-            if((int)msg.id == JOYSTICK_ID) {
-               // pc.printf("RX CANPAcket\r\n");
+            pc.printf("\r\n msg id %d check %d %d \r\n", (int)msg.id, JOYSTICK_ID_L, JOYSTICK_ID_R); 
+            
+            if((int)msg.id == JOYSTICK_ID_L) {
+                
                 int rightStatus = (int)(msg.data[0]>>4)%4;
                 int leftStatus = (int)(msg.data[0]>>2)%4;
-                int centerXStatus = (int)msg.data[0]%4;
-                
+                int centerXStatus = (int)msg.data[0]%4;     
                 int xAxisRaw = (int)(msg.data[0]>>6) + (msg.data[1]<<2);
-                
+                int xl = 1500;
+                if(rightStatus > 0) xl += (xAxisRaw / 2);
+                if(leftStatus > 0) xl -= (xAxisRaw / 2);
+                if(xl > 2000) xl = 2000;
+                if(xl < 1000) xl = 1000;
+                               
                 int forwardStatus = (int)(msg.data[2]>>4)%4;
                 int backwardStatus = (int)(msg.data[2]>>2)%4;
-                int centerYStatus = (int)msg.data[2]%4;
-                
+                int centerYStatus = (int)msg.data[2]%4;             
                 int yAxisRaw = (int)(msg.data[2]>>6) + (msg.data[3]<<2);
+                int yl = 1500;
+                if(forwardStatus > 0) yl += (yAxisRaw / 2);
+                if(backwardStatus > 0) yl -= (yAxisRaw / 2);
+                if(yl > 2000) yl = 2000;
+                if(yl < 1000) yl = 1000;
                 
                 //note button 1 = top left, button 2 = top right
                 int button1 = (int)(msg.data[5]>>6)%4;
                 int button2 = (int)(msg.data[5]>>4)%4;
-                int button3 = (int)(msg.data[5]>>2)%4;
-                int button4 = (int)msg.data[5]%4;
-                
-                //note button 8 = dead-man switch
-                int button5 = (int)(msg.data[6]>>6)%4;
-                int button6 = (int)(msg.data[6]>>4)%4;
-                int button7 = (int)(msg.data[6]>>2)%4;
-                int button8 = (int)msg.data[6]%4;
+                int tl = 1500;
+                if(button2 > 0) tl += 500;
+                if(button1 > 0) tl -= 500;
                 
-                int button9 = (int)(msg.data[7]>>6)%4;
-                int button10 = (int)(msg.data[7]>>4)%4;
-                int button11 = (int)(msg.data[7]>>2)%4;
-                int button12 = (int)msg.data[7]%4;  
+                //front button
+                int button8 = (int)msg.data[6]%4;
+                int fl = 1000;
+                if(button8 > 0) fl += 1000;
                 
-                //pc.printf("RightStatus:%d,LeftStatus:%d,CenterXStatus:%d,XAxisRaw:%d\n\r", rightStatus, leftStatus, centerXStatus, xAxisRaw); 
-                //pc.printf("ForwardStatus:%d,BackwardStatus:%d,CenterYStatus:%d,YAxisRaw:%d\n\r", forwardStatus, backwardStatus, centerYStatus, yAxisRaw); 
+                ppm.setServoPulseDuration_us(3, yl);
+                ppm.setServoPulseDuration_us(4, xl);
+                ppm.setServoPulseDuration_us(5, fl);
+                ppm.setServoPulseDuration_us(7, tl);
                 
-                //pc.printf("Button1:%d,Button2:%d,Button3:%d,Button4:%d\n\r", button1, button2, button3, button4);         
-                //pc.printf("Button5:%d,Button6:%d,Button7:%d,Button8:%d\n\r", button5, button6, button7, button8);
-                //pc.printf("Button1:%d,Button2:%d,Button3:%d,Button3:%d\n\r", button1, button2, button3, button4);
+                
+            } else if((int)msg.id == JOYSTICK_ID_R) {
                 
-                deadMan = button8;
-                //modifying duties
-                if(!deadMan) {
-                    leftDuty = 1500;
-                    rightDuty = 1500;
-                    pc.printf("DeadMan:OFF, LeftDuty:%d,RightDuty:%d\n\r", leftDuty, rightDuty);
-                }
-                else {
-                    int signedXAxis = xAxisRaw;
-                    if(leftStatus==1) {
-                        signedXAxis = -1*signedXAxis;
-                    }
-                    
-                    int signedYAxis = yAxisRaw;
-                    if(backwardStatus==1) {
-                        signedYAxis = -1*signedYAxis;    
-                    }
-                    
-                    //scale between 1000-2000
-                    //left
-                    //leftDuty = 1500 - signedXAxis/2 + signedYAxis/2;
-                    //leftDuty = 1500 -signedXAxis/2 -signedYAxis/2;
-                    leftDuty = 1500 +signedXAxis/2 +signedYAxis/2;
-                    
-                    
-                    if(leftDuty>2000) {
-                        leftDuty = 2000;
-                    }
-                    else if(leftDuty<1000) {
-                        leftDuty = 1000;
-                    }
-                    
-                    //rightDuty = 1500 + signedXAxis/2 + signedYAxis/2;
-                    //rightDuty = 1500 + signedXAxis/2 - signedYAxis/2;
-                    rightDuty = 1500 - signedXAxis/2 + signedYAxis/2;
-                    if(rightDuty>2000) {
-                        rightDuty = 2000;
-                    }
-                    else if(rightDuty<1000) {
-                        rightDuty = 1000;
-                    }
-                    
-                    pc.printf("DeadMan:ON, LeftDuty:%d,RightDuty:%d\n\r", leftDuty, rightDuty);
-                }
+                int rightStatus = (int)(msg.data[0]>>4)%4;
+                int leftStatus = (int)(msg.data[0]>>2)%4;
+                int centerXStatus = (int)msg.data[0]%4;     
+                int xAxisRaw = (int)(msg.data[0]>>6) + (msg.data[1]<<2);
+                int xr = 1500;
+                if(rightStatus > 0) xr += (xAxisRaw / 2);
+                if(leftStatus > 0) xr -= (xAxisRaw / 2);
+                if(xr > 2000) xr = 2000;
+                if(xr < 1000) xr = 1000;
+                               
+                int forwardStatus = (int)(msg.data[2]>>4)%4;
+                int backwardStatus = (int)(msg.data[2]>>2)%4;
+                int centerYStatus = (int)msg.data[2]%4;             
+                int yAxisRaw = (int)(msg.data[2]>>6) + (msg.data[3]<<2);
+                int yr = 1500;
+                if(forwardStatus > 0) yr += (yAxisRaw / 2);
+                if(backwardStatus > 0) yr -= (yAxisRaw / 2);
+                if(yr > 2000) yr = 2000;
+                if(yr < 1000) yr = 1000;
+                
+                //note button 1 = top left, button 2 = top right
+                int button1 = (int)(msg.data[5]>>6)%4;
+                int button2 = (int)(msg.data[5]>>4)%4;
+                int tr = 1500;
+                if(button2 > 0) tr += 500;
+                if(button1 > 0) tr -= 500;
+                
+                //front button
+                int button8 = (int)msg.data[6]%4; 
+                int fr = 1000;
+                if(button8 > 0) fr += 1000;
+                 
+                ppm.setServoPulseDuration_us(1, xr);
+                ppm.setServoPulseDuration_us(2, yr);
+                ppm.setServoPulseDuration_us(6, fr);
+                ppm.setServoPulseDuration_us(8, tr);                    
             }                       
         }   
     }
     
-    //Sends message over the CANbus
-    //- used in operationMode1()
-    void writeCAN()
-    {
-        CANMessage msgOut;
-        msgOut.format = CANExtended;
-        msgOut.id = 0x71;   //messageID
-        msgOut.len = 8;     //length in bytes
-           
-        msgOut.data[0] = (leftDuty>>8)%0xFF;      
-        msgOut.data[1] = leftDuty&0xFF;        
-        msgOut.data[2] = (rightDuty>>8)&0xFF;
-        msgOut.data[3] = rightDuty&0xFF;
-        msgOut.data[4] = deadMan;
-        msgOut.data[5] = 0;
-        msgOut.data[6] = 0;
-        msgOut.data[7] = 0;
-        can.write(msgOut);
-    }
-    
 int main() {
     pc.baud(115200);     
     pc.printf("\r\n Starting CANBus Joystick to ESC PWM \r\n"); 
+                
+    //right stick
+    ppm.setServoPulseDuration_us(1, 1500);
+    ppm.setServoPulseDuration_us(2, 1500);
+    //left stick
+    ppm.setServoPulseDuration_us(3, 1500);
+    ppm.setServoPulseDuration_us(4, 1500);
+    //front buttons   
+    ppm.setServoPulseDuration_us(5, 1000);
+    ppm.setServoPulseDuration_us(6, 1000);
+    //top buttons
+    ppm.setServoPulseDuration_us(7, 1500);
+    ppm.setServoPulseDuration_us(8, 1500);
     
-    //PWM Enable @ 1500 default, 20000 period
-    leftESC.Enable(1500,20000);
-    rightESC.Enable(1500,20000);
-    
+    //start ppm
+    ppm.startServoPpmOutput();
+
     //CANBus @250kb/s
     can.frequency(250000);
    // can.filter(0x70,0x70,CANExtended); //CANbus filtering to only accept 0x70
 
     wait(1.0);
-    
-    ticker.attach(&writeCAN, 0.1); 
 
     while(true) {
-        
+        pc.printf("alive\r\n");
         readCAN();      //read Joystick CANBus and adjust duty
         
-        leftESC.SetPosition(leftDuty); //set duties for left
-        rightESC.SetPosition(rightDuty); //set duties for right
-        
         wait(0.01);
         
         //pc.printf("R: %d, L: %d\r\n", rightDuty, leftDuty);