File content as of revision 3:42a8da1837f7:

#include "mbed.h"
#include "Control.h"
#include "pinmap.h"
#include "SBDBT.h"
void control::setup(){
    sb.setup(0);
}

void control::run()
{
    sb.PS3_JOYSTICK();
    OmniControl();
    ModuleControl();
}
void control::OmniControl()
{    
    Vx = sb.JoyStick_dt[0] * -1;
    Vy = sb.JoyStick_dt[1];
    wl = sb.JoyStick_dt[2];
    
    
    pwm[0]=     (Vx - Vy)*bias[0] + wl;
    pwm[1]=(-1)*(Vx + Vy)*bias[1] + wl;
    pwm[2]=(-1)*(Vx - Vy)*bias[2] + wl;
    pwm[3]=     (Vx + Vy)*bias[3] + wl;
    
    
    for(int i = 0; i < 4; i++){
        if(pwm[i] > 127){
            pwm[i] =127;
        }else if(pwm[i] < -127){
            pwm[i] = -127;
        }
    }
    
    mt1.move(pwm[0]/130.0f);
    mt2.move(pwm[1]/130.0f);
    mt3.move(pwm[2]/130.0f);
    mt4.move(pwm[3]/130.0f);
    mt5.move(pwm[4]/130.0f);
    mt6.move(pwm[5]/130.0f);
}


    int control::CheckButton(int status,int button)
{
    if(status == 1){
        pre_button[button] = 1;
        if(pre_button[button] == 0){
            return 1;
        }
    }else{
        pre_button[button] = 0;
    }
    return 0;
}   

void control::ModuleControl()
{
    if(CheckButton(sb.PS3_TRIANGLE_bit(),TRIANGLE) && s_time > 100){ 
        pwm[4] = 0.40f;
    }else{
        pwm[4] = 0.0f;
    }
    
    if(CheckButton(sb.PS3_CROSS_bit(),CROSS) && s_time > 100){
        pwm[4] = 0.40f;
    }else{
        pwm[4] = 0.0f;
    }
    
    if(CheckButton(sb.PS3_CIRCLE_bit(),CIRCLE)){
        pwm[5] = 0.40f;
    }else{
        pwm[5] = 0.0f;
    }
}