File content as of revision 0:7bc24e8d1591:

#include "Drive.hpp"
#define LED_PIN PF_0

Drive::Drive(PinName CAN_TX, PinName CAN_RX): Rasp(USBTX,USBRX), canMBED(CAN_TX, CAN_RX), LED(LED_PIN), servo(PA_8){
    initCAN();
    //can_Timer.attach(callback(this,&UDP::can_send), 16ms);
    count = 0;
}

void Drive::setVelocity(){
    //LED=0;
    for(int i = 0; i < 4; i++)
    {
        M[i] = 0;
        Rasp::get(M[i], i);
    }
    dribble_set();
    calcWheelVelocity();
    setMotVel();
    can_send();
}

void Drive::dribble_set(){
    if(dribble_power > 0){
        dribble_power = 0.25;
    }
    servo = dribble_power;
}

void Drive::calcWheelVelocity(){
    
    float max;
    double plus;
   
    order.Mot0Order.Vel_short = M[0];
    order.Mot1Order.Vel_short = M[1];
    order.Mot2Order.Vel_short = M[2];
    order.Mot3Order.Vel_short = M[3];
    /*order.Mot0Order.Vel_short = -100;
    order.Mot1Order.Vel_short = -100;
    order.Mot2Order.Vel_short = 100;
    order.Mot3Order.Vel_short = 100;*/
}

void Drive::setMotVel(){
    send_motvel_data[0] = (char)order.Mot0Order.Vel_char.L;
    send_motvel_data[1] = (char)order.Mot0Order.Vel_char.H;
    send_motvel_data[2] = (char)order.Mot1Order.Vel_char.L;
    send_motvel_data[3] = (char)order.Mot1Order.Vel_char.H;
    send_motvel_data[4] = (char)order.Mot2Order.Vel_char.L;
    send_motvel_data[5] = (char)order.Mot2Order.Vel_char.H;
    send_motvel_data[6] = (char)order.Mot3Order.Vel_char.L;
    send_motvel_data[7] = (char)order.Mot3Order.Vel_char.H;
}

void Drive::initCAN(){
    canMBED.frequency(100000);
    
}

void Drive::can_send(){
    canMBED.write(CANMessage(0x1AA,send_motvel_data,8)); 
}

int Drive::masuo_send(){
    return 1;
}