File content as of revision 2:8dd5298ca3a0:

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

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::SolenoidControl()
{
    if(CheckButton(sb.PS3_CIRCLE_bit(),CIRCLE)) {
        sol.write(0,1);
    } else {
        sol.write(0,0);
    }
    if(CheckButton(sb.PS3_SQUARE_bit(),SQUARE)) {
        sol.write(1,1);
    } else {
        sol.write(1,0);
    }
    if(CheckButton(sb.PS3_CROSS_bit(),CROSS)) {
        sol.write(2,1);
    } else {
        sol.write(2,0);
    }
    if(CheckButton(sb.PS3_TRIANGLE_bit(),TRIANGLE)) {
        sol.write(3,1);
    } else {
        sol.write(3,0);
    }
}