オムニの制御プログラムです
Dependencies: mbed QEI PS3_class
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 }
Generated on Fri Jul 15 2022 14:28:53 by
1.7.2