0920

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 }