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 MODE_LED(D6);
00007 DigitalOut flag(D2);
00008 
00009 void set_emg(void)
00010 {
00011     bool start  = ps3.getSTARTState();
00012     bool select = ps3.getSELECTState();
00013 
00014     flag = (select == 0 && start == 1)? 0:flag;
00015     flag = (select == 1 && start == 1)? 1:flag;
00016     flag = (select == 1 && start == 0)? 1:flag;
00017 }
00018 
00019 void get_stick(int *axis,char mode)
00020 {
00021     switch (mode) {
00022         case 'R':
00023             *axis = ps3.getRightJoystickXaxis();
00024             ++axis;
00025             *axis = ps3.getRightJoystickYaxis();
00026             break;
00027         case 'L':
00028             *axis = ps3.getLeftJoystickXaxis();
00029             ++axis;
00030             *axis = ps3.getLeftJoystickYaxis();
00031             break;
00032     }
00033 }
00034 
00035 bool get_but(int butnum)
00036 {
00037     int ps3_cases[12] = {38,36,16,37,32,33,34,35,19,20,17,18};//sign,cross,rl
00038     return ps3.getButtonState(ps3_cases[butnum]);
00039 }
00040 
00041 void set_speed(int *pspeed)
00042 {
00043     *pspeed = (get_but(r2) && get_but(l2))? low:*pspeed;
00044     *pspeed = (ps3.getSTARTState())? fast:*pspeed;
00045     //MODE_LED = (*pspeed == fast)? 1:0;
00046 }
00047 
00048 void set_direct(int *direct)
00049 {
00050     get_stick(Raxis,'R');
00051     get_stick(Laxis,'L');
00052     int Rrad = atan2(double(Raxis[1]), double(Raxis[0]))*double(180/PI);
00053     int Lrad = atan2(double(Laxis[1]), double(Laxis[0]))*double(180/PI);
00054     *direct = SBREAK;
00055     *direct = (Rrad == 0 && Lrad == 0)? 0:*direct;
00056     *direct = (Rrad >= 45 && Rrad <= 135)? 1:*direct;
00057     *direct = (Rrad <= -45 && Rrad >= -135)? 2:*direct;
00058     *direct = (Rrad > -45 && Rrad < 0)||(Rrad > 0 && Rrad < 45)||(Raxis[0] == 63 && Raxis[1] == 0)? 3:*direct;
00059     *direct = (Rrad > 135) || (Rrad > -180 && Rrad < -135)? 4:*direct;
00060     *direct = (Rrad > 0 && get_but(r1))? 5:*direct;
00061     *direct = (Rrad > 0 && get_but(l1))? 6:*direct;
00062 }
00063 
00064 void set_tire(char *fmot, char *bmot, int direct, char mode, int speed)
00065 {
00066     int front = SBREAK;
00067     int back = SBREAK;
00068     front = (speed == low)? pDuty10:pDuty95;
00069     back = (speed == low)?  nDuty10:nDuty95;
00070     switch (direct) {
00071         case 1:
00072             *fmot = front;
00073             *bmot = front;
00074             break;
00075         case 2:
00076             *fmot = back;
00077             *bmot = back;
00078             break;
00079         case 3:
00080             *fmot = (mode == 'R')? back:front;
00081             *bmot = (mode == 'R')? back:front;
00082             break;
00083         case 4:
00084             *fmot = (mode == 'R')? front:back;
00085             *bmot = (mode == 'R')? front:back;
00086             break;
00087         case 5:
00088             if(speed == low) {
00089                 *fmot = (mode == 'R')? front:front+TRANSIT;
00090                 *bmot = (mode == 'R')? front:front+TRANSIT;
00091             } else if(speed == fast) {
00092                 *fmot = (mode == 'R')? front-TRANSIT:front;
00093                 *bmot = (mode == 'R')? front-TRANSIT:front;
00094             }
00095             break;
00096         case 6:
00097             if(speed == low) {
00098                 *fmot = (mode == 'L')? front:front+TRANSIT;
00099                 *bmot = (mode == 'L')? front:front+TRANSIT;
00100             } else if(speed == fast) {
00101                 *fmot = (mode == 'L')? front-TRANSIT:front;
00102                 *bmot = (mode == 'L')? front-TRANSIT:front;
00103             }
00104             break;
00105         case 0:
00106         default :
00107             *fmot = SBREAK;
00108             *bmot = SBREAK;
00109             break;
00110     }
00111 }
00112 
00113 void set_slide(char *Rfmot,char *Lfmot,char *Rbmot,char *Lbmot,int but1,int but2, int speed)
00114 {
00115     int front = SBREAK;
00116     int back = SBREAK;
00117     front = (speed == low)? pDuty10:pDuty95;
00118     back = (speed == low)? nDuty10:nDuty95;
00119     if(get_but(but1) == 1 || get_but(but2) == 1) {
00120         if(get_but(but1) != get_but(but2)) {
00121             int butnum = (get_but(but1) > get_but(but2))? but1:but2;
00122             switch (butnum) {
00123                 case r1:
00124                 case r2:
00125                     *Rfmot = back;
00126                     *Lfmot = front;
00127                     *Rbmot = front;
00128                     *Lbmot = back;
00129                     break;
00130                 case l1:
00131                 case l2:
00132                     *Rfmot = front;
00133                     *Lfmot = back;
00134                     *Rbmot = back;
00135                     *Lbmot = front;
00136                     break;
00137             }
00138         }
00139     } else {
00140         *Rfmot = SBREAK;
00141         *Lfmot = SBREAK;
00142         *Rbmot = SBREAK;
00143         *Lbmot = SBREAK;
00144     }
00145 }
00146 
00147 void set_mot(char *mot,int num1,int num2)
00148 {
00149     *mot = SBREAK;
00150     if(get_but(num1) != get_but(num2)) {
00151         int butnum = (get_but(num1) > get_but(num2))? num1:num2;
00152         switch (butnum) {
00153             case cir:
00154                 *mot = pDuty25;
00155                 break;
00156             case cro:
00157                 *mot = nDuty25;
00158                 break;
00159         }
00160     }
00161 }
00162 
00163 void set_air(char *air, int num1, int num2)
00164 {
00165     if(get_but(num1) != get_but(num2)) {
00166         int butnum = (get_but(num1) > get_but(num2))? num1:num2;
00167         switch (butnum) {
00168             case tri:
00169                 *air = ON;
00170                 break;
00171             case squ:
00172                 *air = OFF;
00173                 break;
00174         }
00175     }
00176 }
00177 
00178 void write_data(char address,char data)
00179 {
00180     wait(0.001);
00181     i2c.start();
00182     i2c.write(address);
00183     i2c.write(data);
00184     i2c.stop();
00185 }
00186 
00187 void format_data(char *data)
00188 {
00189     *data = SBREAK;
00190 }