Diff: main.cpp

Revision:

0:032a5658acdc

Child:

1:b3151b6fe36b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Mar 16 04:00:51 2021 +0000
@@ -0,0 +1,170 @@
+// 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    
+
+//PWM Outputs
+Servo leftESC(D3);
+Servo rightESC(A6);
+
+int leftDuty = 1500;
+int rightDuty = 1500;
+
+bool deadMan = false;
+
+//CANBus                                               
+CAN can(CAN_RD, CAN_TD);
+
+//**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;
+    
+    //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");
+                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 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);
+                
+                //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 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;  
+                
+                //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); 
+                
+                //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);
+                
+                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);
+                }
+            }                       
+        }   
+    }
+    
+    //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"); 
+    
+    //PWM Enable @ 1500 default, 20000 period
+    leftESC.Enable(1500,20000);
+    rightESC.Enable(1500,20000);
+    
+    //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) {
+        
+        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);
+        //wait(1);
+    }
+    
+}
+
+                                   
+
+    
\ No newline at end of file