File content as of revision 0:42d76776f570:

#include "mbed.h"
#include "IMU.h"
#include "Servo.h"
#include "PID.h"
#include "CAN.h"

#define SERVO_DEG 10.0      /* The range in degrees from centre to min or max */ 
#define SERVO_RAN 0.005     /* The pulsewidth range from centre to min or max */

#define DEF_WANT_HEIGHT 200
#define MIN_HEIGHT 50
#define MAX_HEIGHT 1000

#define DEF_PID_KC 1
#define DEF_PID_TAUL 1
#define DEF_PID_TAUD 1
#define DEF_PID_RATE 0.01

#define CAN_IN_ID 0x00000384    // Only channel 900 and 901 will be received   (0000 0000 0000 0000 0000 0011 1000 0100)
#define CAN_MASK 0xFFFFFFFF    // Compare all the bits                         (1111 1111 1111 1111 1111 1111 1111 1110) 

#define CAN_BAUD 500000

IMU     epson(p9,p10);
Servo   actuator(p21);
CAN     can(p30,p29);
Timer   t;
PID     pid(DEF_PID_KC,DEF_PID_TAUL,DEF_PID_TAUD,DEF_PID_RATE);

float   wantedHeight = DEF_WANT_HEIGHT;
float   kc = DEF_PID_KC;
float   tauL = DEF_PID_TAUL;
float   tauD = DEF_PID_TAUD;
float   rate = DEF_PID_RATE;

int     canFilterHandle = 0;

/**
    The received messages must have the following formats:
    Can id: 900
        Byte 0-3: Rate
        Byte 4-7: Kc        
        
    Can id: 901
        Byte 0-1: wantedHeight
    
*/
CANMessage rCanMsg;

void canMsgRes(void){
    can.read(rCanMsg);     // WTF?? Does this put the message in rCanMsg??
    
    switch (rCanMsg.id)
    {
        case 900: 
            // TODO:
            break;
            
        case 901:
            // TODO:
            break;
    }
}

void init(){
    // Init the PID controller
    pid.setOutputLimits(-1*SERVO_DEG, SERVO_DEG);
    pid.setInputLimits(MIN_HEIGHT,MAX_HEIGHT);
    pid.setSetPoint(wantedHeight);                            
    pid.setMode(0);                  //Manual mode
    pid.setProcessValue(wantedHeight);
    pid.reset();
    
    // Init the IMU
    
    
    // Init the Actuators
    
    
    // Init the CANBus
    can.frequency(CAN_BAUD);
    canFilterHandle = can.filter(CAN_IN_ID,CAN_MASK);
    can.attach(&canMsgRes);
    
    // Init the timer
    t.reset();
    
}


int main() {
    // Init all the components
    init();
    
    // The running loop:
    t.start();
    while(1){
        // Read all the sensor data
        
        
        //Wait until the timer runs out
        wait(rate-t.read());
        t.reset();
    }
    
}