0920
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "DualShock.h" 00003 00004 #define ARMSPEED 60 00005 #define SPEED 14 //10->14 00006 #define ROLLSPEED 8 00007 #define ROLLRATE 0.5 00008 00009 #define ARMROLLSPEED 40 00010 00011 Serial DS_serial(PC_10, PC_11); 00012 Serial pc(SERIAL_TX, SERIAL_RX); 00013 00014 PwmOut MD1PWM1(PA_0); 00015 PwmOut MD1PWM2(PA_1); 00016 PwmOut MD2PWM1(PA_6); 00017 PwmOut MD2PWM2(PC_7); 00018 PwmOut MD3PWM1(PB_15); 00019 PwmOut MD3PWM2(PB_13); 00020 00021 DigitalOut MD1CW1(PB_0); 00022 DigitalOut MD1CCW1(PC_1); 00023 DigitalOut MD1DIS1(PA_4); 00024 DigitalOut MD1CW2(PC_2); 00025 DigitalOut MD1CCW2(PC_0); 00026 DigitalOut MD1DIS2(PC_3); 00027 00028 DigitalOut MD2CW1(PA_8); 00029 DigitalOut MD2CCW1(PA_10); 00030 DigitalOut MD2DIS1(PB_10); 00031 00032 DigitalOut MD2CW2(PB_4); 00033 DigitalOut MD2CCW2(PB_3); 00034 DigitalOut MD2DIS2(PB_5); 00035 DigitalOut MD3CW1(PB_12); 00036 DigitalOut MD3CCW1(PB_14); 00037 DigitalOut MD3DIS1(PA_11); 00038 DigitalOut MD3CW2(PA_12); 00039 DigitalOut MD3CCW2(PC_4); 00040 DigitalOut MD3DIS2(PC_5); 00041 00042 DigitalOut magnet1(PB_8); 00043 DigitalOut magnet2(PB_9); 00044 DigitalOut magnet3(PC_6); 00045 00046 DigitalIn limit1(PA_13); 00047 DigitalIn limit2(PA_14); 00048 DigitalIn limit3(PA_15); 00049 00050 int main() { 00051 DS_serial.baud(115200); //通信速度設定 00052 InitDS(&DS_serial); //受信データ用変数を初期化する 00053 DS_serial.attach(&getDSdata, Serial::RxIrq); //「受信したら割り込みして」の宣言 00054 MD1DIS1 = 0; 00055 MD1DIS2 = 0; 00056 MD2DIS1 = 0; 00057 MD2DIS2 = 0; 00058 MD3DIS1 = 0; 00059 MD3DIS2 = 0; 00060 magnet1 = 0; 00061 magnet2 = 0; 00062 magnet3 = 0; 00063 MD1PWM1.period_us(100);//アーム開閉 00064 MD1PWM2.period_us(100);//右前オムニ 00065 MD2PWM1.period_us(100);//リフト上下 00066 MD2PWM2.period_us(100);//アーム回転 00067 MD3PWM1.period_us(100);//左前オムニ 00068 MD3PWM2.period_us(100);//後方オムニ 00069 int speedgear; 00070 speedgear = 1; 00071 int gear; 00072 gear = 1; 00073 00074 float root3=1.73205; 00075 00076 float rightfront; 00077 float leftfront; 00078 float back; 00079 00080 int lim1, lim2, lim3 = {0}; 00081 00082 while(1){ 00083 lim1 = limit1; 00084 lim2 = limit2; 00085 lim3 = limit3; 00086 00087 00088 if(hDS.BUTTON.RIGHT == 1){ 00089 MD1PWM1.pulsewidth_us(ARMSPEED); 00090 MD1CW1 = 0; 00091 MD1CCW1 = 1; 00092 } 00093 else if(hDS.BUTTON.LEFT == 1){ 00094 MD1PWM1.pulsewidth_us(ARMSPEED); 00095 MD1CW1 = 1; 00096 MD1CCW1 = 0; 00097 } 00098 else{ 00099 MD1PWM1.pulsewidth_us(0); 00100 MD1CW1 = 0; 00101 MD1CCW1 = 0; 00102 }//アーム開閉 00103 00104 00105 00106 00107 if(hDS.BUTTON.R2 == 1){ 00108 speedgear = 3; 00109 gear = 2; 00110 } 00111 if(hDS.BUTTON.L2 == 1){ 00112 speedgear = 1; 00113 gear = 1; 00114 }//スピード調整 00115 00116 /* 00117 if(hDS.BUTTON.L1 == 1){ 00118 MD1PWM2.pulsewidth_us(speedgear*ROLLSPEED); 00119 MD1CW2 = 1; 00120 MD1CCW2 = 0; 00121 MD3PWM1.pulsewidth_us(speedgear*ROLLSPEED); 00122 MD3CW1 = 0; 00123 MD3CCW1 = 1; 00124 MD3PWM2.pulsewidth_us(speedgear*ROLLSPEED); 00125 MD3CW2 = 0; 00126 MD3CCW2 = 1; 00127 }//右回転 00128 else if(hDS.BUTTON.R1 == 1){ 00129 MD1PWM2.pulsewidth_us(speedgear*ROLLSPEED); 00130 MD1CW2 = 0; 00131 MD1CCW2 = 1; 00132 MD3PWM1.pulsewidth_us(speedgear*ROLLSPEED); 00133 MD3CW1 = 1; 00134 MD3CCW1 = 0; 00135 MD3PWM2.pulsewidth_us(speedgear*ROLLSPEED); 00136 MD3CW2 = 1; 00137 MD3CCW2 = 0; 00138 }//左回転 00139 */ 00140 00141 00142 00143 if( (float)hDS.ANALOG.LY > (float)-0.1 && (float)hDS.ANALOG.LY < (float)0.1 && (float)hDS.ANALOG.LX > (float)-0.1 && (float)hDS.ANALOG.LX < (float)0.1 && hDS.BUTTON.L1 == 0 && hDS.BUTTON.R1 == 0){ 00144 MD1PWM2.pulsewidth_us(0); 00145 MD1CW2 = 0; 00146 MD1CCW2 = 0; 00147 00148 MD3PWM1.pulsewidth_us(0); 00149 MD3CW1 = 0; 00150 MD3CCW1 = 0; 00151 00152 MD3PWM2.pulsewidth_us(0); 00153 MD3CW2 = 0; 00154 MD3CCW2 = 0; 00155 } 00156 else{ 00157 00158 rightfront = hDS.ANALOG.LX*-2/2 + hDS.ANALOG.LY*2/root3 + (int)hDS.BUTTON.L1 * ROLLRATE + (int)hDS.BUTTON.R1 * -ROLLRATE; 00159 leftfront = hDS.ANALOG.LX*-2/2+hDS.ANALOG.LY*-2/root3 + (int)hDS.BUTTON.L1 * ROLLRATE + (int)hDS.BUTTON.R1 * -ROLLRATE; 00160 back = hDS.ANALOG.LX*2 + (int)hDS.BUTTON.L1 * ROLLRATE + (int)hDS.BUTTON.R1 * -ROLLRATE; 00161 00162 00163 if(rightfront < 0){ 00164 MD1PWM2.pulsewidth_us((int)(rightfront*speedgear*SPEED*-1)); 00165 MD1CW2 = 0; 00166 MD1CCW2 = 1; 00167 } 00168 else{ 00169 MD1PWM2.pulsewidth_us((int)(rightfront*speedgear*SPEED)); 00170 MD1CW2 = 1; 00171 MD1CCW2 = 0; 00172 } 00173 if(leftfront < 0){ 00174 MD3PWM1.pulsewidth_us((int)(leftfront*speedgear*SPEED*-1)); 00175 MD3CW1 = 1; 00176 MD3CCW1 = 0; 00177 } 00178 else{ 00179 MD3PWM1.pulsewidth_us((int)(leftfront*speedgear*SPEED)); 00180 MD3CW1 = 0; 00181 MD3CCW1 = 1; 00182 } 00183 if(back < 0){ 00184 MD3PWM2.pulsewidth_us((int)(back*speedgear*SPEED*-1)); 00185 MD3CW2 = 1; 00186 MD3CCW2 = 0; 00187 } 00188 else{ 00189 MD3PWM2.pulsewidth_us((int)(back*speedgear*SPEED)); 00190 MD3CW2 = 0; 00191 MD3CCW2 = 1; 00192 }//移動 00193 } 00194 00195 00196 00197 00198 if((float)hDS.ANALOG.RY < (float)-0.1){ 00199 MD2PWM2.pulsewidth_us(hDS.ANALOG.RY*ARMROLLSPEED*-1); 00200 MD2CW2 = 0; 00201 MD2CCW2 = 1; 00202 } 00203 else if ((float)hDS.ANALOG.RY > (float)0.1){ 00204 MD2PWM2.pulsewidth_us(hDS.ANALOG.RY*ARMROLLSPEED); 00205 MD2CW2 = 1; 00206 MD2CCW2 = 0; 00207 } 00208 else{ 00209 MD2PWM2.pulsewidth_us(0); 00210 MD2CW2 = 0; 00211 MD2CCW2 = 0; 00212 } 00213 //アーム回転 00214 00215 if(hDS.BUTTON.CIRCLE == 1){ 00216 magnet1 = 1; 00217 magnet2 = 1; 00218 magnet3 = 1; 00219 } 00220 if(hDS.BUTTON.SQUARE == 1){ 00221 magnet1 = 0; 00222 magnet2 = 0; 00223 magnet3 = 0; 00224 }//電磁石 00225 00226 if(hDS.BUTTON.UP == 1){ 00227 MD2PWM1.pulsewidth_us(gear*50); 00228 MD2CW1 = 0; 00229 MD2CCW1 = 1; 00230 } 00231 if(hDS.BUTTON.DOWN == 1){ 00232 MD2PWM1.pulsewidth_us(gear*45); 00233 MD2CW1 = 1; 00234 MD2CCW1 = 0; 00235 } 00236 if(hDS.BUTTON.DOWN == 0 && hDS.BUTTON.UP == 0){ 00237 MD2PWM1.pulsewidth_us(0); 00238 MD2CW1 = 0; 00239 MD2CCW1 = 0; 00240 }//lift 00241 00242 if(hDS.BUTTON.CROSS == 1){ 00243 MD1PWM1.pulsewidth_us(0); 00244 MD1CW1 = 0; 00245 MD1CCW1 = 0; 00246 MD1PWM2.pulsewidth_us(0); 00247 MD1CW2 = 0; 00248 MD1CCW2 = 0; 00249 MD2PWM1.pulsewidth_us(0); 00250 MD2CW1 = 0; 00251 MD2CCW1 = 0; 00252 MD2PWM2.pulsewidth_us(0); 00253 MD2CW2 = 0; 00254 MD2CCW2 = 0; 00255 MD3PWM1.pulsewidth_us(0); 00256 MD3CW1 = 0; 00257 MD3CCW1 = 0; 00258 MD3PWM2.pulsewidth_us(0); 00259 MD3CW2 = 0; 00260 MD3CCW2 = 0; 00261 magnet1 = 0; 00262 magnet2 = 0; 00263 magnet3 = 0; 00264 while(1){ 00265 } 00266 }//強制終了 00267 /* 00268 pc.printf("/t rightfront%d/n",(int)rightfront*gear*SPEED); 00269 pc.printf("/t leftfront%d/n",(int)leftfront*gear*SPEED); 00270 pc.printf("/t back%d/n",(int)back*gear*SPEED); 00271 00272 pc.printf("%d\t %d\t\t %d\t%d\r\n",hDS.BUTTON.L2, hDS.BUTTON.UP, hDS.BUTTON.TRIANGLE, hDS.BUTTON.R2); 00273 pc.printf("%d\t%d %d\t\t%d %d\t%d\r\n",hDS.BUTTON.L1, hDS.BUTTON.LEFT, hDS.BUTTON.RIGHT, hDS.BUTTON.SQUARE, hDS.BUTTON.CIRCLE, hDS.BUTTON.R1); 00274 pc.printf("\t %d\t %d/%d\t %d\r\n",hDS.BUTTON.DOWN, hDS.BUTTON.SELECT, hDS.BUTTON.START, hDS.BUTTON.CROSS); 00275 pc.printf("LX:%.3f,LY:%.3f\tRX:%.3f,RY:%.3f\r\n",hDS.ANALOG.LX, hDS.ANALOG.LY, hDS.ANALOG.RX, hDS.ANALOG.RY); 00276 pc.printf("\r\n"); 00277 pc.printf("11CW=%d/11CCW=%d\t\t12CW=%d/12CCW=%d\t\r\n", (int)MD1CW1, (int)MD1CCW1, (int)MD1CW2, (int)MD1CCW2); 00278 pc.printf("21CW=%d/21CCW=%d\t\t22CW=%d/22CCW=%d\t\r\n", (int)MD2CW1, (int)MD2CCW1, (int)MD2CW2, (int)MD2CCW2); 00279 pc.printf("31CW=%d/31CCW=%d\t\t32CW=%d/32CCW=%d\t\r\n", (int)MD3CW1, (int)MD3CCW1, (int)MD3CW2, (int)MD3CCW2); 00280 pc.printf("LIM=%d%d%d\r\n", lim1, lim2, lim3); 00281 pc.printf("\r\n"); 00282 pc.printf("\r\n"); 00283 */ 00284 00285 00286 } 00287 }
Generated on Mon Jul 18 2022 11:21:59 by
1.7.2