File content as of revision 0:032a5658acdc:

// 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);
    }
    
}