File content as of revision 0:dfd7b420339f:

/*
 * ======================
 * VS-C3/VS-RCV3 PIN
 * ======================
 * 1:NC  2:NC
 * 3:DAT 4:CMD
 * 5:SEL 6:CLK
 * 7:+5~7V 8:NC
 * 9:+3V 10:GND
 *
 */
 
#include "mbed.h"
#include "PS_PAD.h"

class Motor{
    PwmOut a, b;
public:
    Motor(PinName A_term, PinName B_term) : a(A_term), b(B_term) {
        a.period(0.001);
        b.period(0.001);
    }
    
    Motor& operator=(double duty) {
        if(duty > 0){
            a = duty < 1 ? duty : 1;
            b = 0;
        }else {
            a = 0;
            b = duty > -1 ? -duty : -1;
        }
        return *this;
    }
};


PS_PAD vsc3(PA_7, PA_6, PA_5, PA_4); //MOSI, MISO, SCK, SS

DigitalIn lim[7] = {PD_2, PD_3, PD_4, PD_5, PD_6, PD_7, PG_9};
Motor rmotor(PE_9, PE_11);
Motor lmotor(PE_13, PE_14);
Motor sxmotor(PA_0, PA_1);
Motor tsmotor(PA_2, PA_3);
Motor trmotor(PC_6, PC_7);
Motor tlmotor(PC_8, PC_9);

int main() {
    vsc3.init();
    double movey = 0, turn = 0;
    printf("START\n");
    
    while(1) {
        vsc3.poll();
        if (vsc3.read(PS_PAD::PAD_TOP)){
            movey = -1;
        }else if(vsc3.read(PS_PAD::PAD_BOTTOM)){
            movey = 1;
        }else if(abs(vsc3.read(PS_PAD::ANALOG_LY)) > 10){
            movey = (vsc3.read(PS_PAD::ANALOG_LY)) / 128.0;
        }else{
            movey = 0;
        }
               
        if (vsc3.read(PS_PAD::PAD_RIGHT)){
            turn = -1;
        }else if(PS_PAD::PAD_LEFT){
            turn = 1;
        }else if(abs(vsc3.read(PS_PAD::ANALOG_LX)) > 10){
            turn = (vsc3.read(PS_PAD::ANALOG_LX)) / 128.0;
        }else{
            turn = 0;
        }
        
        rmotor = -movey+turn;      //move
        lmotor = -movey-turn;
        
        if (vsc3.read(PS_PAD::PAD_CIRCLE) && lim[0] == 0){        //limitswitch
            sxmotor = 1;
        }else if(vsc3.read(PS_PAD::PAD_X) && lim[1] == 0){
            sxmotor = -1;
        }else if(vsc3.read(PS_PAD::PAD_CIRCLE) && vsc3.read(PS_PAD::PAD_X)){
            sxmotor = 0;
        }else{
            sxmotor = 0;
        }
        
        if (vsc3.read(PS_PAD::PAD_TRIANGLE) && lim[2] == 0){
            tsmotor = 1;
        }else if(vsc3.read(PS_PAD::PAD_SQUARE) && lim[3] == 0){
            tsmotor = -1;
        }else if(vsc3.read(PS_PAD::PAD_TRIANGLE) && vsc3.read(PS_PAD::PAD_SQUARE)){
            tsmotor = 0;
        }else{
            tsmotor = 0;
        }        
        
        if (vsc3.read(PS_PAD::PAD_R1) && lim[4] == 0){
            trmotor = 1;
        }else if(vsc3.read(PS_PAD::PAD_R2) && lim[5] == 0){
            trmotor = -1;
        }else if(vsc3.read(PS_PAD::PAD_R1) && vsc3.read(PS_PAD::PAD_R2)){
            trmotor = 0;
        }else{
            trmotor = 0;
        }
                
        if (vsc3.read(PS_PAD::PAD_L1) && lim[6] == 0){
            tlmotor = 1;
        }else if(vsc3.read(PS_PAD::PAD_L2) && lim[7] == 0){
            tlmotor = -1;
        }else if(vsc3.read(PS_PAD::PAD_L1) && vsc3.read(PS_PAD::PAD_L2)){
            tlmotor = 0;
        }else{
            tlmotor = 0;
        }
        
        wait(0.05);
    }
}