File content as of revision 0:c725bf9715c4:

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

#define MAX_PULSE 2300
#define MIN_PULSE 700
#define MAX_DEGREE 180.0f
#define MIN_DEGREE 0.0f

#define GRIPPER_ID 0
#define JAMMER_ID 1

#define RED_ID 0
#define BLUE_ID 1
#define PURPLE_ID 2


#ifndef M_PI
#define M_PI 3.14159265359f
#endif

//float __float_reg[64];                                                          // Floats stored in flash
//int __int_reg[256];                                                             // Ints stored in flash.  Includes position sensor calibration lookup table

CAN can(PA_11,PA_12);
Ticker can_ticker;
Timer timer;
PwmOut led_blue(PB_4);
PwmOut led_red(PB_5);
PwmOut gripper(PA_1);
PwmOut jammer(PA_4);

ControllerStruct controller;

void moveServo(uint8_t id,double degree);
void readCANData();
void offLED();
void shineLED(uint8_t color_id,double brightness);


int main()
{
    //controller.v_bus = 6.0f;    //6[V]
    //controller.mode = 0;
    can.frequency(1000000);
    can.filter(CAN_ID, 0xFFF, CANStandard, 0);
    //msg.len = 8;
    can.attach(&readCANData);

    //can_ticker.attach(&readCANData,0.01);
    timer.start();
    gripper.period_ms(20);
    jammer.period_ms(20);
    int kai=0;
    double position_now=0;
    double pre_loop_time=0;
    while(1) {
        /*
        double velocity = controller.kp*(controller.p_des-position_now)+controller.kd*controller.v_des;
        double loop_time=timer.read();
        double dt=loop_time-pre_loop_time;
        position_now += velocity*dt;
        moveServo(GRIPPER_ID,position_now*M_PI/180.0f);
        moveServo(JAMMER_ID,position_now*M_PI/180.0f);
        pre_loop_time=loop_time;
        */
        
        moveServo(GRIPPER_ID,controller.p_des*180.0f/M_PI);
        moveServo(JAMMER_ID,controller.p_des*180.0f/M_PI);
        //offLED();
        kai++;
        if(kai>1000) {
            //printf("303:time=%f,position=%f\r\n",loop_time,position_now);
            printf("p:%f,v:%f,kp:%f,kd:%f\r\n",controller.p_des,controller.v_des,controller.kp,controller.kd);
            kai=0;
        }
    }
}

void offLED()
{
    led_blue=0;
    led_red=0;
}

void onLED(uint8_t color_id,double brightness)
{
    if(brightness>1)brightness=1;
    else if(brightness<0)brightness=0;
    switch(color_id) {
        case RED_ID:
            led_blue=0;
            led_red=brightness;
            break;
        case BLUE_ID:
            led_blue=brightness;
            led_red=0;
            break;
        case PURPLE_ID:
            led_blue=brightness;
            led_red=brightness;
            break;
        default:
            break;
    }
}

void moveServo(uint8_t id,double degree)
{
    int pulse=(MAX_PULSE-MIN_PULSE)*degree/(MAX_DEGREE-MIN_DEGREE)+MIN_PULSE;
    if(id==0)gripper.pulsewidth_us(pulse);
    else if(id==1)jammer.pulsewidth_us(pulse);
}

void readCANData()
{
    CANMessage can_msg;
    //offLED();
    if(can.read(can_msg)) {
        if(can_msg.id==CAN_ID) {
            onLED(BLUE_ID,0.2);
            unpack_cmd(can_msg,&controller);
        }
    }
    offLED();
}