File content as of revision 1:a636d553c70a:

#include "mbed.h"
#include "CAN_com.h"
#include "structs.h"

#define PULSE_MAX 2450  // us
#define PULSE_MIN 550   //us  
#define PERIOD 20       // us
#define POS_MAX 3.14159265359f  // rad
#define POS_MIN 0.0f            // rad
#define M_PI 3.14159265359f  // rad

#define CAN_ID 4

CAN can(PA_11,PA_12);
Ticker can_ticker;
Timer timer;

PwmOut led_blue(PB_4);
PwmOut led_red(PB_5);

PwmOut servo1(PA_1);
PwmOut servo2(PA_3);

ControllerStruct controller;

void moveServo(uint8_t id,double degree);
void readCANData();

bool received = false;

int main()
{
    can.frequency(1000000);
    can.filter(CAN_ID, 0xFFF, CANStandard, 0);
    can.attach(&readCANData);

    servo1.period_ms(PERIOD);
    servo2.period_ms(PERIOD);

    while(1) {
        if (received == true){
            double gripper_pos = controller.p_des;
            int pulse1 = (PULSE_MAX - PULSE_MIN) * gripper_pos / (POS_MAX - POS_MIN) + PULSE_MIN;
            int pulse2 = (PULSE_MAX - PULSE_MIN) * (M_PI - gripper_pos) / (POS_MAX - POS_MIN) + PULSE_MIN;
            servo1.pulsewidth_us(pulse1);
            servo2.pulsewidth_us(pulse2);
            wait(0.01);
        }
    }
}

void readCANData()
{
    CANMessage can_msg;
    if(can.read(can_msg)) {
        if(can_msg.id==CAN_ID) {
            unpack_cmd(can_msg,&controller);
            received = true;
        }
    }
}