File content as of revision 0:ed8bf5304fbb:

#include "mbed.h"
#include "Control.h"
#include "SBDBT.h"
#include "pinmap.h"

void control::setup(){
    for(int i = 0; i <= 4; i++){
         bias[i]=0.0f;
    }

}

void control::run(){
    
    sb.PS3_JOYSTICK();
    OmniControl();

}

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);
}

void control::SolenoidControl()
{   if(sb.PS3_CIRCLE_bit() == 1){
        if(pre_button sircle[0]==0){
            sol.write(0,1);
    }else{
            pre_button sircle==1;
            sol.write(0,0);
            
        }
    } 
    if(sb.PS3_Triangle_bit() == 1){
        if(pre_button triangle[0]==0){
            sol.write(1,1);
    }else{
            pre_button triangle==1;
            sol.write(1,0);
            
        }
    } 
    if(sb.PS3_square_bit() == 1){
        if(pre_button square[0]==0){
            sol.write(2,1);
    }else{
            pre_button square==1;
            sol.write(2,0);
            
        }
    }
    if(sb.PS3_cross_bit() == 1){
        if(pre_button cross[0]==0){
            sol.write(3,1);
    }else{
            pre_button cross==1;
            sol.write(3,0);
            
        }
    }    
}