自動機の手動プログラム

Dependencies:   mbed QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Help.cpp Source File

Help.cpp

00001 #include "Help.h"
00002 #include "PS3.h"
00003 
00004 PS3 ps3(PA_0,PA_1);
00005 I2C i2c(D14,D15);
00006 DigitalOut POWER_LED(D5);
00007 DigitalOut flag(D6);
00008 DigitalIn arm_mae(A3);
00009 DigitalIn arm_usiro(A2);
00010 DigitalIn drop_mae(D3);
00011 DigitalIn drop_usiro(D2);
00012 int motion_phase;
00013 
00014 void set_gpio(void)
00015 {
00016     POWER_LED = 1;
00017     arm_mae.mode(PullUp);
00018     arm_usiro.mode(PullUp);
00019     drop_mae.mode(PullUp);
00020     drop_usiro.mode(PullUp);
00021 }
00022 
00023 void set_emg(void)
00024 {
00025     bool start  = ps3.getSTARTState();
00026     bool select = ps3.getSELECTState();
00027 
00028     flag = (select == 0 && start == 1)? 0:flag;
00029     flag = (select == 1 && start == 1)? 1:flag;
00030     flag = (select == 1 && start == 0)? 1:flag;
00031 }
00032 
00033 void get_stick(int *axis,char mode)
00034 {
00035     switch (mode) {
00036         case 'R':
00037             *axis = ps3.getRightJoystickXaxis();
00038             ++axis;
00039             *axis = ps3.getRightJoystickYaxis();
00040             break;
00041         case 'L':
00042             *axis = ps3.getLeftJoystickXaxis();
00043             ++axis;
00044             *axis = ps3.getLeftJoystickYaxis();
00045             break;
00046     }
00047 }
00048 
00049 bool get_but(int butnum)
00050 {
00051     int ps3_cases[12] = {38,36,16,37,32,33,34,35,19,20,17,18};//sign,cross,rl
00052     return ps3.getButtonState(ps3_cases[butnum]);
00053 }
00054 
00055 void set_speed(int *pspeed)
00056 {
00057     *pspeed = (get_but(r2) && get_but(l2))? low:*pspeed;
00058     *pspeed = (ps3.getSTARTState())? fast:*pspeed;
00059 }
00060 
00061 void set_direct(int *direct)
00062 {
00063     get_stick(Raxis,'R');
00064     get_stick(Laxis,'L');
00065     int Rrad = atan2(double(Raxis[1]), double(Raxis[0]))*double(180/PI);
00066     int Lrad = atan2(double(Laxis[1]), double(Laxis[0]))*double(180/PI);
00067     *direct = SBREAK;
00068     *direct = (Rrad == 0 && Lrad == 0)? 0:*direct;
00069     *direct = (Rrad >= 45 && Rrad <= 135)? 1:*direct;
00070     *direct = (Rrad <= -45 && Rrad >= -135)? 2:*direct;
00071     *direct = (Rrad > -45 && Rrad < 0)||(Rrad > 0 && Rrad < 45)||(Raxis[0] == 63 && Raxis[1] == 0)? 3:*direct;
00072     *direct = (Rrad > 135) || (Rrad > -180 && Rrad < -135)? 4:*direct;
00073     *direct = (Rrad > 0 && get_but(r1))? 5:*direct;
00074     *direct = (Rrad > 0 && get_but(l1))? 6:*direct;
00075 }
00076 
00077 void set_tire(char *fmot, char *bmot, int direct, char mode, int speed)
00078 {
00079     int front = SBREAK;
00080     int back = SBREAK;
00081     front = (speed == low)? pDuty50:pDuty70;
00082     back = (speed == low)?  nDuty50:nDuty70;
00083     switch (direct) {
00084         case 1:
00085             *fmot = front;
00086             *bmot = front;
00087             break;
00088         case 2:
00089             *fmot = back;
00090             *bmot = back;
00091             break;
00092         case 3:
00093             *fmot = (mode == 'R')? back:front;
00094             *bmot = (mode == 'R')? back:front;
00095             break;
00096         case 4:
00097             *fmot = (mode == 'R')? front:back;
00098             *bmot = (mode == 'R')? front:back;
00099             break;
00100         case 5:
00101             if(speed == low) {
00102                 *fmot = (mode == 'R')? front:front+TRANSIT;
00103                 *bmot = (mode == 'R')? front:front+TRANSIT;
00104             } else if(speed == fast) {
00105                 *fmot = (mode == 'R')? front-TRANSIT:front;
00106                 *bmot = (mode == 'R')? front-TRANSIT:front;
00107             }
00108             break;
00109         case 6:
00110             if(speed == low) {
00111                 *fmot = (mode == 'L')? front:front+TRANSIT;
00112                 *bmot = (mode == 'L')? front:front+TRANSIT;
00113             } else if(speed == fast) {
00114                 *fmot = (mode == 'L')? front-TRANSIT:front;
00115                 *bmot = (mode == 'L')? front-TRANSIT:front;
00116             }
00117             break;
00118         case 0:
00119         default :
00120             *fmot = SBREAK;
00121             *bmot = SBREAK;
00122             break;
00123     }
00124 }
00125 
00126 void set_slide(char *Rfmot,char *Lfmot,char *Rbmot,char *Lbmot,int but1,int but2, int speed)
00127 {
00128     int front = SBREAK;
00129     int back = SBREAK;
00130     front = (speed == low)? pDuty50:pDuty70;
00131     back = (speed == low)? nDuty50:nDuty70;
00132     if(get_but(but1) ^ get_but(but2)) {
00133         int butnum = (get_but(but1) > get_but(but2))? but1:but2;
00134         switch (butnum) {
00135             case r1:
00136             case r2:
00137                 *Rfmot = back;
00138                 *Lfmot = front;
00139                 *Rbmot = front;
00140                 *Lbmot = back;
00141                 break;
00142             case l1:
00143             case l2:
00144                 *Rfmot = front;
00145                 *Lfmot = back;
00146                 *Rbmot = back;
00147                 *Lbmot = front;
00148                 break;
00149         }
00150     } else {
00151         *Rfmot = SBREAK;
00152         *Lfmot = SBREAK;
00153         *Rbmot = SBREAK;
00154         *Lbmot = SBREAK;
00155     }
00156 }
00157 
00158 void set_mot(char *mot,int num1,int num2)
00159 {
00160     *mot = SBREAK;
00161     if(get_but(num1) != get_but(num2)) {
00162         int butnum = (get_but(num1) > get_but(num2))? num1:num2;
00163         switch (butnum) {
00164             case cir:
00165                 *mot = (arm_mae == 1)? pDuty85:SBREAK;
00166                 break;
00167             case cro:
00168                 *mot = (arm_usiro == 1)? nDuty85:SBREAK;
00169                 break;
00170         }
00171     }
00172 }
00173 
00174 void set_arm(char *mot, int num, int pulse)
00175 {
00176     //押しっぱなしで動くやつ(まだましさだまさし)
00177     //motion_phase = get_but(num);
00178 
00179     //一回ボタン押したら動くやつ(危険)
00180     if(arm_mae == 0 && arm_usiro == 1) {
00181         motion_phase = get_but(num);
00182     }
00183 
00184     switch (motion_phase) {
00185         case 0:
00186             *mot = SBREAK;
00187             motion_phase = 0;
00188             break;
00189         case 1://前進->減速
00190             *mot = (pulse < 2000)? pDuty90:pDuty40;
00191             motion_phase = (pulse < 2000)? 1:2;
00192             break;
00193         case 2://前進->後進
00194             //*mot = (arm_usiro == 1)? pDuty40:nDuty80;
00195             if(arm_usiro == 1) {
00196                 *mot = pDuty40;
00197                 motion_phase = 2;
00198             } else {
00199                 char send_data[1] =  {SBREAK};
00200                 motion_phase = 3;
00201                 i2c.write(Arm_add, send_data,     1, false);
00202                 wait(2);
00203             }
00204             //motion_phase = (arm_usiro == 1)? 2:3;
00205             break;
00206         case 3://後進->減速
00207             *mot = (pulse > 1600)? nDuty90:nDuty40;
00208             motion_phase = (pulse > 1600)? 3:4;
00209             break;
00210         case 4://後進->停止
00211             *mot = (arm_mae == 1)? nDuty40:SBREAK;
00212             motion_phase = (arm_mae == 1)? 4:0;
00213             break;
00214         default:
00215             *mot = SBREAK;
00216             motion_phase = 0;
00217             break;
00218 
00219     }
00220 }
00221 
00222 void set_drop(char *mot, int num1,int num2)
00223 {
00224     *mot = SBREAK;
00225     if(get_but(num1) != get_but(num2)) {
00226         int butnum = (get_but(num1) > get_but(num2))? num1:num2;
00227         switch (butnum) {
00228             case squ:
00229                 *mot = (drop_mae == 1)? pDuty85:SBREAK;
00230                 break;
00231             case cro:
00232                 *mot = (drop_usiro == 1)? nDuty85:SBREAK;
00233                 break;
00234         }
00235     }
00236 }
00237 
00238 void set_sheet(char *mot,char mode, int num1, int num2)
00239 {
00240     *mot = SBREAK;
00241     if(get_but(num1) != get_but(num2)) {
00242         int butnum = (get_but(num1) > get_but(num2))? num1:num2;
00243         switch (butnum) {
00244             case up:
00245                 *mot = (mode == 'R')? pDuty85:nDuty85;
00246                 break;
00247             case dow:
00248                 *mot = (mode == 'R')? nDuty85:pDuty85;
00249                 break;
00250         }
00251     }
00252 
00253 }
00254 
00255 void write_data(char address,char data)
00256 {
00257     char send_data[1] =  {data};
00258     i2c.write(address, send_data,     1, false);
00259 }