File content as of revision 1:9c7744271d72:

#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_R1_bit()==1) {
        if(sb.PS3_CIRCLE_bit() == 1&&pre_PS3_R1_bit==0) {
            if(pre_button_circle[0]==0/*&&sb.PS3_R1_bit()==1*/) {
                sol.write(0,1);
            } else {
                pre_button_circle[0]=1;//sb.PS3_R1_bit()==0;
                sol.write(0,0);
            }
        }
        if(sb.PS3_TRIANGLE_bit() == 1&&pre_PS3_R1_bit==0) {
            if(pre_button_triangle[0]==0/*&&sb.PS3_R1_bit()==1*/) {
                sol.write(1,1);
            } else {
                pre_button_triangle[0]=1;//sb.PS3_R1_bit()==0;
                sol.write(1,0);
            }
            if(sb.PS3_SQUARE_bit() == 1&&pre_PS3_R1_bit==0) {
                if(pre_button_square[0]==0/*&&sb.PS3_R1_bit()==1*/) {
                    sol.write(2,1);
                } else {
                    pre_button_square[0]=1;//sb.PS3_R1_bit()==0;
                    sol.write(2,0);

                }
                if(sb.PS3_CROSS_bit() == 1&&pre_PS3_R1_bit==0) {
                    if(pre_button_cross[0]==0) {
                        sol.write(3,1);
                    } else {
                        pre_button_cross[0]=1;//;sb.PS3_R1_bit()==0;
                        sol.write(3,0);
                    }
                }
            pre_PS3_R1_bit=1;
            }else{
                pre_PS3_R1_bit=0;
            }
        }
    }
}