File content as of revision 16:55c180a4338c:

#include "Utils.h"
#include "USBHost.h"
#include "hci.h"
#include "ps3.h"
#include "User.h"

#include "mbed.h"
#include "math.h"
#include "move3wheel.h"


int RSX,RSY,LSX,LSY,BSU,BSL;
/*DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);*/

SPI spi(p5,p6,p7);
DigitalOut cs(p8);
int data1 = 0;
int a;  //aは定数
double receive_data/*,true_data*/;
double now_angle,target_angle,out;

PwmOut motor0CW(p21);  //0番motor 外から見て時計回り
PwmOut motor0CCW(p22);  //0番motor 反時計回り
PwmOut motor1CW(p23);  //1番motor
PwmOut motor1CCW(p24);  //1番motor
PwmOut motor2CW(p26);  //2番motor
PwmOut motor2CCW(p25);  //2番motor


void motor(double cw0,double ccw0,double cw1,double ccw1,double cw2,double ccw2)
{
    motor0CW = cw0;
    motor0CCW = ccw0;
    motor1CW = cw1;
    motor1CCW = ccw1;
    motor2CW = cw2;
    motor2CCW = ccw2;
}



void UserLoopSetting()
{
    spi.format(16,3);
    spi.frequency(1000000);

    motor0CW.period_us(50);
    motor0CCW.period_us(50);
    motor1CW.period_us(50);
    motor1CCW.period_us(50);
    motor2CW.period_us(50);
    motor2CCW.period_us(50);
}

void UserLoop(char n,const u8* data)
{
    u16 ButtonState;
    if(n==0) { //有線Ps3USB.cpp
        RSX = ((ps3report*)data)->RightStickX;
        RSY = ((ps3report*)data)->RightStickY;
        LSX = ((ps3report*)data)->LeftStickX;
        LSY = ((ps3report*)data)->LeftStickY;
        BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
        BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
        //ボタンの処理
        ButtonState =  ((ps3report*)data)->ButtonState;
    } else {//無線TestShell.cpp
        RSX = ((ps3report*)(data + 1))->RightStickX;
        RSY = ((ps3report*)(data + 1))->RightStickY;
        LSX = ((ps3report*)(data + 1))->LeftStickX;
        LSY = ((ps3report*)(data + 1))->LeftStickY;
        BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
        BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
        //ボタンの処理
        ButtonState =  ((ps3report*)(data + 1))->ButtonState;
    }

///////////////////////////ここからmotorのプログラム///////////////////////////////

    /*int a = (ButtonState >> BUTTONRIGHT)&1;
    if(a == 0)led4=0;
    else led4=1;*/

    //printf("RSX = %d, RSY = %d\r\n", RSX, RSY);
    if(RSX >=100 && RSX <150 && RSY >=100 && RSY <150
            && (((ButtonState >> BUTTONRIGHT)&1) == 0) && (((ButtonState >> BUTTONDOWN)&1) == 0)) {  //ニュートラルポジション
        motor(0,0,0,0,0,0);
        /*led1 = 1;
        led2 = 0;
        led3 = 0;
        led4 = 0;*/


    } else if(RSX >=100 && RSX <150 && RSY <100) { // ↑ 移動
    
      if((ButtonState >> BUTTONR1)&1 == 1){
        CalMotorOut(0,0.13,0);
        
      }else{
        CalMotorOut(0,0.3,0);  //走らせたい速さを入れる(X方向,y方向,回転) 回転方向は上から見て反時計回りが正
      } 
      
        if(now_angle > 135 && now_angle < 225){
        motor(0,0,0,GetMotorOut(1),-GetMotorOut(2),0); 
        
        }else if(now_angle > 225 && now_angle < 315){
            if((ButtonState >> BUTTONR1)&1 == 1){
              CalMotorOut(0.13,0,0);
        
            }else{
              CalMotorOut(0.3,0,0);
            }
        motor(0,GetMotorOut(0),GetMotorOut(1),0,GetMotorOut(2),0);
            
        }else{
        motor(0,0,GetMotorOut(1),0,0,-GetMotorOut(2));  //1番motorはCW方向、2番motorはCCW方向に回す
    
        }   

    } else if(RSX >=100 && RSX <150 && RSY >=150) { // ↓ 移動
        if((ButtonState >> BUTTONR1)&1 == 1){
        CalMotorOut(0,0.13,0);
        
        }else{
        CalMotorOut(0,0.3,0);
        }
        
        if(now_angle > 135 && now_angle < 225){
        motor(0,0,GetMotorOut(1),0,0,-GetMotorOut(2));
            
        }else if(now_angle > 225 && now_angle < 315){
           if((ButtonState >> BUTTONR1)&1 == 1){
             CalMotorOut(0.13,0,0);
        
           }else{
             CalMotorOut(0.3,0,0);
           }
        motor(GetMotorOut(0),0,0,GetMotorOut(1),0,GetMotorOut(2));
        
        }else{
        motor(0,0,0,GetMotorOut(1),-GetMotorOut(2),0);  //前進と逆の方向に回す
    
        }
        
    } else if(RSX >=150 && RSY >=100 && RSY <150) { // → 移動
        if((ButtonState >> BUTTONR1)&1 == 1){
         CalMotorOut(0.13,0,0);
        
        }else{
         CalMotorOut(0.3,0,0);
        }
        
        if(now_angle > 135 && now_angle < 225){
        motor(0,GetMotorOut(0),GetMotorOut(1),0,GetMotorOut(2),0);
            
        }else if(now_angle > 225 && now_angle < 315){
          if((ButtonState >> BUTTONR1)&1 == 1){
            CalMotorOut(0,0.13,0);
        
          }else{
            CalMotorOut(0,0.3,0);
          }
        motor(0,0,GetMotorOut(1),0,0,-GetMotorOut(2));  
            
        }else{
        motor(GetMotorOut(0),0,0,GetMotorOut(1),0,GetMotorOut(2));
    
        }
        
    } else if(RSX <100 && RSY >=100 && RSY <150) {  // ← 移動
        if((ButtonState >> BUTTONR1)&1 == 1){
         CalMotorOut(0.13,0,0);
        
        }else{
         CalMotorOut(0.3,0,0);
        }
        
         if(now_angle > 135 && now_angle < 225){
         motor(GetMotorOut(0),0,0,GetMotorOut(1),0,GetMotorOut(2));
        
         }else if(now_angle > 225 && now_angle < 315){
           if((ButtonState >> BUTTONR1)&1 == 1){
            CalMotorOut(0,0.13,0);
        
           }else{
            CalMotorOut(0,0.3,0);
           }
         motor(0,0,0,GetMotorOut(1),-GetMotorOut(2),0); 
             
         }else{
         motor(0,GetMotorOut(0),GetMotorOut(1),0,GetMotorOut(2),0);
        
        }

    } else if(RSX < 100 && RSY <100) {  //  左上移動
        if((ButtonState >> BUTTONR1)&1 == 1){
         CalMotorOut(0.1,0.1,0);
        
        }else{
         CalMotorOut(0.2,0.2,0);
        }
        
        if(now_angle > 135 && now_angle < 225){
        motor(GetMotorOut(0),0,0,GetMotorOut(1),-GetMotorOut(2),0);
            
        }else if(now_angle > 225 && now_angle < 315){
          if((ButtonState >> BUTTONR1)&1 == 1){
           CalMotorOut(0.1,-0.1,0);
        
          }else{
           CalMotorOut(0.2,-0.2,0);
          }
        motor(0,GetMotorOut(0),0,-GetMotorOut(1),GetMotorOut(2),0);
       
        }else{ 
        motor(0,GetMotorOut(0),GetMotorOut(1),0,0,-GetMotorOut(2));
        
        }

    } else if(RSX >= 150 && RSY <100) {  //  右上移動
        if((ButtonState >> BUTTONR1)&1 == 1){
         CalMotorOut(-0.1,0.1,0);
        
        }else{
         CalMotorOut(-0.2,0.2,0);
        }
        
        if(now_angle > 135 && now_angle < 225){
        motor(0,-GetMotorOut(0),0,GetMotorOut(1),-GetMotorOut(2),0);
            
        }else if(now_angle > 225 && now_angle < 315){
         if((ButtonState >> BUTTONR1)&1 == 1){
          CalMotorOut(0.1,0.1,0);
        
         }else{
          CalMotorOut(0.2,0.2,0);
         }        
        motor(0,GetMotorOut(0),GetMotorOut(1),0,0,-GetMotorOut(2));
        
        }else{
        motor(-GetMotorOut(0),0,GetMotorOut(1),0,0,-GetMotorOut(2));
    
        }

    } else if(RSX <100 && RSY >=150) {  //  左下移動
       if((ButtonState >> BUTTONR1)&1 == 1){
        CalMotorOut(0.1,-0.1,0);
        
      }else{
        CalMotorOut(0.2,-0.2,0);
      }
      
        if(now_angle > 135 && now_angle < 225){
        motor(GetMotorOut(0),0,-GetMotorOut(1),0,0,GetMotorOut(2));
            
        }else if(now_angle > 225 && now_angle < 315){
         if((ButtonState >> BUTTONR1)&1 == 1){
          CalMotorOut(-0.1,-0.1,0);
        
         }else{
          CalMotorOut(-0.2,-0.2,0);
         }
        motor(-GetMotorOut(0),0,0,-GetMotorOut(1),GetMotorOut(2),0);
            
        }else{
        motor(0,GetMotorOut(0),0,-GetMotorOut(1),GetMotorOut(2),0);
        
        }

    } else if(RSX >= 150 && RSY >=150) {  //  右下移動
        if((ButtonState >> BUTTONR1)&1 == 1){
        CalMotorOut(-0.1,-0.1,0);
        
        }else{
        CalMotorOut(-0.2,-0.2,0);
        }
        
        if(now_angle > 135 && now_angle < 225){
        motor(0,-GetMotorOut(0),-GetMotorOut(1),0,0,GetMotorOut(2));
            
        }else if(now_angle > 225 && now_angle < 315){
         if((ButtonState >> BUTTONR1)&1 == 1){
          CalMotorOut(-0.1,-0.1,0);
        
         }else{
          CalMotorOut(-0.2,0.2,0);
         }
        motor(-GetMotorOut(0),0,GetMotorOut(1),0,0,-GetMotorOut(2));
        
        }else{
        motor(-GetMotorOut(0),0,0,-GetMotorOut(1),GetMotorOut(2),0);
        
        }

    }
    
    if(RSX >=100 && RSX <150 && RSY >=100 && RSY <150) {  //右スティックニュートラルポジション
        if(LSX >=100 && LSX <150 ) {  //左スティックニュートラルポジション
            motor(0,0,0,0,0,0);

        } else if(LSX >=150 ) {  //右回転
            motor(0,0.2,0,0.2,0,0.2);

        } else if(LSX <100 ) {  //左回転
            motor(0.2,0,0.2,0,0.2,0);

        } else {
            motor(0,0,0,0,0,0);
        }
    }
////////////////////////////ここからヌクレオのプログラム/////////////////////////////

    if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {    //servo1
        wait(0.1);
        if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {
            data1=1;

        }
    }


    if((ButtonState >> BUTTONCIRCLE)&1 == 1) {    //servo2
        wait(0.1);
        if((ButtonState >> BUTTONCIRCLE)&1 == 1) {
            data1=2;
        }
    }


    if((ButtonState >> BUTTONCROSS)&1 == 1) {    //servo3
        wait(0.1);
        if((ButtonState >> BUTTONCROSS)&1 == 1) {
            data1=3;
        }
    }


    if((ButtonState >> BUTTONSQUARE)&1 == 1) {    //servo4
        wait(0.1);
        if((ButtonState >> BUTTONSQUARE)&1 == 1) {
            data1=4;
        }
    }


    if((ButtonState >> BUTTONLEFT)&1 == 1) {    //エアシリンダー
        wait(0.1);
        if((ButtonState >> BUTTONLEFT)&1 == 1) {
            data1=5;
        }
    }

////////////////////////ここからジャイロの角度指定プログラム//////////////////////////

    if(receive_data >= 0) {  //反時計回りに0~359°となるよう修正
        a = receive_data / 360;
        now_angle = receive_data - (360 * a);  //現在の角度
    } else {
        a = receive_data / 360;
        now_angle = - receive_data + (360 * a);
    }


    /*true_data = receive_data - 65536;
    } else {
    true_data = receive_data;
    }*/


    //now_angle = true_data;  //現在の角度
    //out = 0.01 * (target_angle - now_angle);  //P(目標の角度 - 現在の角度) Pは自分で決めた定数

    if((ButtonState >> BUTTONRIGHT)&1 == 1) {
        wait(0.1);
        if((ButtonState >> BUTTONRIGHT)&1 == 1) {
            target_angle = 270;
            out = 0.01 * (target_angle - now_angle);

            //printf("%f\r\n",now_angle);
            //printf("%f\r\n",out);
            if(now_angle >= 268 && now_angle <= 272){
                motor(0,0,0,0,0,0);           
            
            } else if(out > 1.8 || out <= -0.3) {  //0～89°と270～359°のときは時計回りに回転
                motor(0.3,0,0.3,0,0.3,0);
                //printf("cw 0.3\r\n");

            } else if(out > -0.3 && out <= 0) {
                motor(-out,0,-out,0,-out,0);
                //printf("cw -out\r\n");

            } else if(out > 0 && out <= 0.3) {  //90～269°のときは反時計回りに回転
                motor(0,out,0,out,0,out);
                //printf("ccw out\r\n");

            } else if(out <= 1.8 && out > 0.3) {
                motor(0,0.3,0,0.3,0,0.3);
                //printf("ccw 0.3\r\n");
            }
        }
    }

    if((ButtonState >> BUTTONDOWN)&1 == 1) {
        wait(0.1);
        if((ButtonState >> BUTTONDOWN)&1 == 1) {
            target_angle = 180;
            out = 0.01 * (target_angle - now_angle);

            if(now_angle >= 178 && now_angle <= 182){
                motor(0,0,0,0,0,0);
                
            } else if(out > 0.3) {  //0～178°のときは反時計回りに回転
                motor(0,0.3,0,0.3,0,0.3);

            } else if(out <= 0.3 && out > 0) {
                motor(0,out,0,out,0,out);

            } else if(out <= 0 && out > -0.3) {  //180～359°のときは時計回りに回転
                motor(-out,0,-out,0,-out,0);

            } else if(out <= -0.3) {
                motor(0.3,0,0.3,0,0.3,0);
            }
        }
    }
    
    if((ButtonState >> BUTTONUP)&1 == 1) {
        wait(0.1);
        if((ButtonState >> BUTTONUP)&1 == 1) {
            if(0 <= now_angle && now_angle < 180) {
                target_angle = 0;
                out = -0.01 * (target_angle - now_angle);

                if(now_angle <= 1) {
                    motor(0,0,0,0,0,0);

                } else if(out > 0.3) {  //0～179°のときは時計回りに回転
                    motor(0.3,0,0.3,0,0.3,0);

                } else if(out <= 0.3 && out > 0) {
                    motor(out,0,out,0,out,0);
                }
            }

            if(180 <= now_angle && now_angle< 360) {
                target_angle = 360;
                out = 0.01 * (target_angle - now_angle);

                if(now_angle >= 359) {
                    motor(0,0,0,0,0,0);

                } else if(out > 0.3) {  //180～359°のときは反時計回りに回転
                    motor(0,0.3,0,0.3,0,0.3);

                } else if(out <= 0.3 && out > 0) {
                    motor(0,out,0,out,0,out);
                }
            }
        }
    }
        

    cs=0;
    receive_data = spi.write(data1);

    cs=1;
    data1=0;



}