オムニの制御プログラムです

Dependencies:   mbed QEI PS3_class

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "PS3.h"
00004 
00005 
00006 //エンコーダー個数
00007 #define encoder_MAX 4
00008 
00009 //PS3ボタン個数
00010 #define ps3_MAX 3
00011 
00012 //ショートブレーキデータ
00013 #define SBREAK 0x80
00014 
00015 //i2c用ピン
00016 I2C i2c(PB_9,PB_8);
00017 
00018 //SBDBT用通信ポート
00019 PS3 ps3(D8,D2);
00020 
00021 //PCとの通信用
00022 Serial pc(USBTX,USBRX);//D14,D15
00023 
00024 //LED
00025 DigitalOut green(PB_7);     //電源確認
00026 DigitalOut yellow(PC_2);    //i2c確認
00027 DigitalOut red(PC_3);       //動作確認
00028 DigitalOut blue(PC_13);     //12V確認
00029 
00030 //12V停止ピン
00031 DigitalOut emergency(D10);
00032 
00033 
00034 //エンコーダー用ピン
00035 QEI wheel_RF(PA_8,  PA_6,  NC, 600);    //encoder1
00036 QEI wheel_RB(PB_14, PB_13, NC, 600);    //encoder2
00037 QEI wheel_LF(PB_1 , PB_15, NC, 600);    //encoder3
00038 QEI wheel_LB(PA_12, PA_11, NC, 600);    //encoder4
00039 
00040 //パルス、rpm取得用
00041 Ticker get_rpm;
00042 
00043 //MD用アドレスとデータ格納用変数
00044 char data_RF[1]={};
00045 char data_RB[1]={};
00046 char data_LF[1]={};
00047 char data_LB[1]={};
00048 
00049 //初期化MDデータ
00050 char init_data[1]={0x80};
00051 
00052 //PS3ジョイスティックデータ格納用変数
00053 int ps3_stick[4]={};         //0:右ジョイスティックY軸 1:右ジョイスティックX軸 2:左ジョイスティックY軸 3:左ジョイスティックX軸
00054 
00055 //PS3ボタンデータ格納変数
00056 bool ps3_button[ps3_MAX]={};
00057 
00058 //rpm,パルス、合計パルス格納変数
00059 double rpm[encoder_MAX]={};
00060 int pulse[encoder_MAX]={};
00061 int sum_pulse[encoder_MAX]={};
00062 
00063 //関数プロトタイプ宣言
00064 void initialization();
00065 void stop_12V();
00066 void get_data();
00067 void change_data();
00068 void send_data();
00069 void change_rpm();
00070 void print();
00071 
00072 int main(){
00073     
00074     initialization();
00075     
00076     get_rpm.attach_us(&change_rpm,40000);
00077     
00078     while(true)
00079     {
00080         get_data();
00081         stop_12V();
00082         change_data();
00083         send_data();
00084         print();
00085     }
00086 }
00087 
00088 //初期化
00089 void initialization(){
00090     
00091     bool i2c_check[4]={1,1,1,1};    //0:成功 0以外:失敗
00092     
00093     green=1;
00094     blue=1;
00095     pc.baud(115200);
00096     i2c.frequency(100000);
00097     
00098     //エンコーダーリセット
00099     wheel_RF.reset();
00100     wheel_RB.reset();
00101     wheel_LF.reset();
00102     wheel_LB.reset();
00103     
00104     i2c_check[0]=i2c.write(0x10,init_data,1);
00105     i2c_check[1]=i2c.write(0x12,init_data,1);
00106     i2c_check[2]=i2c.write(0x14,init_data,1);
00107     i2c_check[3]=i2c.write(0x16,init_data,1);
00108     
00109     if(i2c_check[0]!=0||i2c_check[1]!=0||i2c_check[2]!=0||i2c_check[3]!=0)
00110         yellow=0;
00111     else
00112         yellow=1;
00113         
00114     emergency=0;
00115     
00116 }    
00117 
00118 //緊急停止
00119 void stop_12V(){
00120     
00121     static bool old_data=0;
00122     static bool flag=0;
00123     
00124     if(old_data!=ps3_button[0])
00125     {
00126         old_data=ps3_button[0];
00127         if(ps3_button[0]==1)
00128         {
00129             if(flag==0)
00130             {
00131                 flag=1;
00132                 emergency=1;
00133                 blue=0;
00134             }
00135             else
00136             {
00137                 flag=0;
00138                 emergency=0;
00139                 blue=1;
00140             }
00141         }
00142     }
00143      
00144 }
00145 
00146 //rpm,合計パルス取得取得
00147 void change_rpm(){
00148     
00149     //パルス取得
00150     pulse[0]=wheel_RF.getPulses();
00151     pulse[1]=wheel_RB.getPulses();
00152     pulse[2]=wheel_LF.getPulses();
00153     pulse[3]=wheel_LB.getPulses();
00154     
00155     //エンコーダーリセット
00156     wheel_RF.reset();
00157     wheel_RB.reset();
00158     wheel_LF.reset();
00159     wheel_LB.reset();
00160     
00161     //合計パルス取得
00162     for(int i=0;i<encoder_MAX;i++){
00163         sum_pulse[i]=sum_pulse[i]+pulse[i];
00164     }
00165     
00166     //rpm取得
00167     for(int j=0;j<encoder_MAX;j++){
00168         rpm[j]=60*25*(double)pulse[j]/1200;
00169     }
00170     
00171 }
00172 
00173 //PS3データ取得
00174 void get_data(){
00175     
00176     ps3_stick[0]=ps3.getRightJoystickYaxis();
00177     ps3_stick[1]=ps3.getRightJoystickXaxis();
00178     ps3_stick[2]=ps3.getLeftJoystickYaxis();
00179     ps3_stick[3]=ps3.getLeftJoystickXaxis();
00180     ps3_button[0]=ps3.getSELECTState();
00181     ps3_button[1]=ps3.getButtonState(R1);
00182     ps3_button[2]=ps3.getButtonState(L1);
00183     
00184     if(ps3_stick[0]==0&&ps3_stick[1]==0&&ps3_stick[2]==0&&ps3_stick[3]==0&&ps3_button[0]==0&&ps3_button[1]==0&&ps3_button[2]==0)
00185         red=0;
00186     else
00187         red=1;
00188     
00189 }
00190 
00191 //MDデータ変更
00192 void change_data(){
00193     
00194     
00195     if(ps3_stick[0]>30){
00196         //前進
00197         data_RF[0]=0xff; data_RB[0]=0x00; data_LF[0]=0x00; data_LB[0]=0xff;
00198     }
00199     else if(ps3_stick[0]<-30){
00200         //後進
00201         data_RF[0]=0x00; data_RB[0]=0xff; data_LF[0]=0xff; data_LB[0]=0x00;
00202     }
00203     else if(ps3_stick[1]>30){
00204         //右
00205         data_RF[0]=0x00; data_RB[0]=0x00; data_LF[0]=0x00; data_LB[0]=0x00;
00206     }
00207     else if(ps3_stick[1]<-30)
00208     {
00209         //左
00210         data_RF[0]=0xff; data_RB[0]=0xff; data_LF[0]=0xff; data_LB[0]=0xff;
00211     }
00212     else if((ps3_stick[2]>30)&&(ps3_stick[3]>30))
00213     {
00214         //右斜め上
00215         data_RF[0]=0x80; data_RB[0]=0x00; data_LF[0]=0x00; data_LB[0]=0x80;
00216         
00217     }
00218     else if((ps3_stick[2]<-30)&&(ps3_stick[3]>30))
00219     {
00220         //右斜め下
00221         data_RF[0]=0x00; data_RB[0]=0x80; data_LF[0]=0x80; data_LB[0]=0x00;
00222         
00223     }
00224     else if((ps3_stick[2]>30)&&(ps3_stick[3]<-30))
00225     {
00226         //左斜め上
00227         data_RF[0]=0xff; data_RB[0]=0x80; data_LF[0]=0x80; data_LB[0]=0xff;
00228         
00229     }
00230     else if((ps3_stick[2]<-30)&&(ps3_stick[3]<-30))
00231     {
00232         //左斜め下
00233         data_RF[0]=0x80; data_RB[0]=0xff; data_LF[0]=0xff; data_LB[0]=0x80;
00234         
00235     }
00236     else if(ps3_button[1]==1)
00237     {
00238         //右旋回
00239         data_RF[0]=0x00; data_RB[0]=0x00; data_LF[0]=0xff; data_LB[0]=0xff;
00240     }
00241     else if(ps3_button[2]==1)
00242     {
00243         //左旋回
00244         data_RF[0]=0xff; data_RB[0]=0xff; data_LF[0]=0x00; data_LB[0]=0x00;
00245     }
00246     else
00247     {
00248         //停止
00249         data_RF[0]=SBREAK; data_RB[0]=SBREAK; data_LF[0]=SBREAK; data_LB[0]=SBREAK;
00250     }
00251 
00252 }
00253 
00254 void send_data(){
00255     
00256     bool i2c_check[4]={1,1,1,1};           //0:成功 0以外:失敗
00257     
00258     i2c_check[0]=i2c.write(0x10,data_RF,1);
00259     i2c_check[1]=i2c.write(0x12,data_RB,1);
00260     i2c_check[2]=i2c.write(0x14,data_LF,1);
00261     i2c_check[3]=i2c.write(0x16,data_LB,1);
00262     
00263     if(i2c_check[0]!=0||i2c_check[1]!=0||i2c_check[2]!=0||i2c_check[3]!=0)
00264         yellow=0;
00265     else
00266         yellow=1;
00267     
00268 }
00269 
00270 //表示用関数
00271 void print(){
00272     
00273     //ps3データ
00274     //pc.printf("RY:%d Rx:%d select:%d R1:%d L1:%d\n",ps3_stick[0],ps3_stick[1],ps3_button[0],ps3_button[1],ps3_button[2]);
00275     //合計パルス,rpm
00276     pc.printf("RF_pulse:%d RB_pulse:%d LF_pulse:%d LB_pulse:%d RF_rpm:%lf RB_rpm:%lf LF_rpm:%lf LB_rpm:%lf\n",sum_pulse[0],sum_pulse[1],sum_pulse[2],sum_pulse[3],rpm[0],rpm[1],rpm[2],rpm[3]);
00277     
00278 }