main.cpp

00001 #include "mbed.h"
00002 #include "ikarashiMDC.h"
00003 #include "pinconfig.h"
00004 #include "omni_wheel.h"
00005 #include "Servo.h"
00006 #include "math.h"
00007 #include "SerialMultiByte.h"
00008 #include "PID.h"
00009 #include "R1370.h"
00010 #include "OmniPosition.h"
00011 #include "FEP_RX22.h"
00012 #include "cmath"
00013  
00014 #define PI 3.141592653589
00015 #define maxSpeed 0.4
00016  
00017 DigitalIn userButton(USER_BUTTON);
00018 AnalogIn VOLUME(A0);
00019  
00020  
00021 FEP_RX22 mycon(fepTX, fepRX, fepad);
00022 Serial pc(pcTX, pcRX, 115200);
00023 Serial RS485(mdcTX,mdcRX,115200);
00024  
00025  
00026 DigitalOut stop(em_stop);
00027  
00028 SerialMultiByte rcv(sub2TX,sub2RX);
00029  
00030 ikarashiMDC motor[] = {
00031     ikarashiMDC(0,0,SM,&RS485), //asi
00032     ikarashiMDC(0,1,SM,&RS485), //asi
00033     ikarashiMDC(0,2,SM,&RS485), //asi
00034     ikarashiMDC(0,3,SM,&RS485), //asi
00035     ikarashiMDC(1,0,SM,&RS485), //全体昇降1
00036     ikarashiMDC(1,1,SM,&RS485), //全体昇降2
00037     ikarashiMDC(1,2,SM,&RS485), //フジモン機構
00038     ikarashiMDC(1,3,SM,&RS485), //フジモン機構
00039     ikarashiMDC(2,0,SM,&RS485), //井上左昇降
00040     ikarashiMDC(2,1,SM,&RS485), //井上右昇降
00041     ikarashiMDC(2,2,SM,&RS485), //井上左前後
00042     ikarashiMDC(2,3,SM,&RS485), //井上右前後
00043 };
00044  
00045 Servo pwm_imput1(BLDC1);//ブラシレス宣言
00046 Servo pwm_imput2(BLDC2);
00047 Servo pwm_imput3(BLDC3);
00048 // angとanglとangleの変数とクラス名がある。気をつけよう*********************重要重要***********************
00049 uint8_t b[16];
00050 int16_t stick[4];
00051 int16_t trigger[4];
00052 int16_t volume[3];
00053 uint8_t toggle[4];
00054 uint8_t timeout;
00055 uint8_t data[128];
00056 int pw;
00057 
00058 double speed[12];
00059 double engine[4];
00060 double spin_power;
00061 double posiX , posiY , posiTheta;
00062 
00063 int TargetAngle = 0;
00064 
00065 int sum_1 = 0;
00066 int sum_2 = 0;
00067 
00068 double bldcspeed = 0.8;
00069 
00070 int16_t angle[4] = {0};//エンコーダ受信格納
00071 uint8_t pulse[8] = {0};
00072  
00073 OmniWheel omni(4);
00074 OmniPosition posi(sub1TX, sub1RX);
00075  
00076 PID angl(10.0, 5.0, 0.0000005, 0.01);
00077  
00078 //プロトタイプ宣言
00079 void chassis();         //機体自体の制御
00080 void spin(double ang);  //PID
00081 int  pm(double num);    //正負判定
00082  
00083 Timer t;
00084  
00085 int main(void){
00086      t.start();
00087     
00088     rcv.setHeaders(0xff,0xff);
00089     rcv.startReceive(4); //SerialMultiByte受信
00090     
00091     mycon.StartReceive(); //コントローラー受信・宣言
00092  
00093     omni.wheel[0].setRadian(PI * 1.0 / 4.0);
00094     omni.wheel[1].setRadian(PI * 3.0 / 4.0);
00095     omni.wheel[2].setRadian(PI * 5.0 / 4.0);
00096     omni.wheel[3].setRadian(PI * 7.0 / 4.0);
00097     
00098     angl.setInputLimits(-180,180);
00099     angl.setOutputLimits(-0.4,0.4);
00100     angl.setBias(0);
00101     angl.setMode(1);
00102     angl.setSetPoint(0);
00103     while(1){
00104         stop = toggle[0];
00105         chassis();
00106     }
00107     
00108 }
00109  
00110 void chassis(){
00111         
00112  #if ControllerMode   
00113         for (int i=0; i<16; i++) b[i] = mycon.getButton(i);
00114         for (int i=0; i<4; i++) stick[i] = mycon.getStick(i);
00115         for (int i=0; i<2; i++) trigger[i] = mycon.getTrigger(i);
00116         
00117 //        for (int i=0; i<16; i++) pc.printf("%d ", b[i]);
00118 //        pc.printf(" | ");
00119 //        for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]);
00120 //        pc.printf(" | ");
00121 //        for (int i=0; i<2; i++) pc.printf("%3d ", trigger[i]);
00122 //        pc.printf(" | ");
00123 #else
00124 
00125         /*ジャイロ読み取り*/
00126         /*足回りのXY座標は多分使わないので無くてもよし。*/
00127         posiX = posi.getX();
00128         posiY = posi.getY();
00129         posiTheta = posi.getTheta()*(180.0/M_PI);//OmniPositionライブラリが弧度法で角度をつけていたため60分法に変換
00130         pc.printf("x:%5.2f Y:%5.2f theta:%0.3f | ",posiX, posiY,posiTheta);
00131         
00132         mycon.getData(data);
00133 /*        for (int i=0, tmp=1; i<8; i++) {
00134             pw = pow((float)2,i);
00135             b[i] = (int)((data[0] & tmp)/pw);
00136             pc.printf("%d ", b[i]);
00137             tmp *= 2;
00138         }
00139         for (int i=8, tmp=1, j=0; i<16; i++, j++) {
00140             pw = pow((float)2,j);
00141             b[i] = (int)((data[1] & tmp)/pw);
00142             pc.printf("%d ", b[i]);
00143             tmp *= 2;
00144         }
00145 */
00146         for (int i=0, tmp=1; i<8; i++) {
00147             pw = pow((float)2,i);
00148             b[i] = (int)((data[0] & tmp)/pw);
00149 //          pc.printf("%d ", b[i]);          上のポインタの式が分からんので無理やり10進数に変える
00150             sum_1 += b[i]*pow((float)2,i+1);
00151             tmp *= 2;
00152         }
00153         pc.printf("%3d ",sum_1);
00154         /*初期化*/
00155         sum_1 = 0;
00156         
00157         for (int i=8, tmp=1, j=0; i<16; i++, j++) {
00158             pw = pow((float)2,j);
00159             b[i] = (int)((data[1] & tmp)/pw);
00160 //          pc.printf("%d ", b[i]);             
00161             sum_2 += b[i]*pow((float)2,i-7);
00162             tmp *= 2;
00163         }
00164         pc.printf("%3d ",sum_2);
00165         /*初期化*/
00166         sum_2 = 0;
00167         pc.printf(" | ");
00168         
00169         for (int i=0; i<4; i++) {
00170             stick[i] = (data[i+2] - 128)*(-1);
00171             pc.printf("%3d ", stick[i]);
00172         }
00173         pc.printf(" | ");
00174         
00175         for (int i=0; i<2; i++) {
00176             trigger[i] = data[i+6];
00177             pc.printf("%3d ", trigger[i]);
00178         }
00179         pc.printf(" | ");
00180         
00181         for (int i=0; i<3; i++) {
00182             volume[i] = data[i+9];
00183             pc.printf("%3d ", volume[i]);
00184         }
00185         pc.printf(" | ");
00186         
00187         for (int i=0; i<4; i++) {
00188             toggle[i] = data[i+12];
00189             pc.printf("%3d ", toggle[i]);
00190         }
00191         pc.printf(" | ");
00192         
00193         timeout = data[8];
00194         pc.printf("%3d ", timeout);
00195         pc.printf(" | ");
00196         
00197         
00198 #endif
00199         if (mycon.getStatus()) pc.printf("receive");
00200         else pc.printf("anything error..."); 
00201         
00202         rcv.getData(pulse); //エンコーダ受信
00203         for(int i=0,j=0;i<4;i++,j+=2){
00204             angle[i] = pulse[j] << 8 | pulse[j+1];  
00205         }
00206         pc.printf("| ENC1:%d | ENC2:%d | ENC3:%d | ENC4:%d |",angle[0],angle[1],angle[2],angle[3]);
00207         if(abs(stick[2])<10){
00208             pc.printf("TA:%.2f pid:%.2f T-p:%.2f",TargetAngle,-angl.compute(),TargetAngle-posiTheta);
00209         }
00210         pc.printf("\r\n");
00211  
00212         //ブラシレスモーター
00213         pwm_imput1 = ((double)volume[0]/255.0)*bldcspeed*toggle[1];
00214  
00215         pwm_imput2 = ((double)volume[1]/255.0)*bldcspeed*toggle[2];
00216  
00217         pwm_imput3 = ((double)volume[2]/255.0)*bldcspeed*toggle[3];
00218 //        pc.printf("servo:%.2f | servo2:%.2f | servo3:%.2f\r\n",
00219 //                  ((double)volume[0]/255.0)*bldcspeed,((double)volume[1]/255.0)*bldcspeed,((double)volume[2]/255.0)*bldcspeed);
00220         
00221         /*井上機構ON,OFF*/
00222         if(toggle[1]){
00223             speed[10] = -0.9;
00224         }else{
00225             speed[10] = 0;
00226         }
00227         if(toggle[1] && b[15]){
00228             speed[10] = 0.4;
00229         }
00230         
00231         if(toggle[3]){
00232             speed[11] = -0.9;
00233         }else{
00234             speed[11] = 0;
00235         }
00236         if(toggle[3] && b[15]){
00237             speed[11] = 0.4;
00238         }
00239         
00240         /*フジモン機構*/
00241         if(toggle[2]){
00242             speed[6] = 0.6;
00243             speed[7] = 0.6;   
00244         }else{
00245             speed[6] = 0;
00246             speed[7] = 0;            
00247         }
00248         
00249         /*展開昇降*/
00250         if(b[5] != 0){
00251             speed[4] = 0.5;
00252             speed[5] = 0.5;
00253         }else if(b[4] != 0){
00254             speed[4] = -0.35;
00255             speed[5] = -0.35;
00256         }else{
00257             speed[4] = 0;
00258             speed[5] = 0;
00259         }
00260         //機構昇降
00261         if(b[9]){
00262             speed[8] = -0.35;
00263         }else
00264         if(b[13]){    
00265             speed[8] = 0.4;
00266         }
00267         if(b[11]){
00268             speed[9] = -0.35;
00269         }else
00270         if(b[14]){    
00271             speed[9] = 0.4;
00272         }
00273         if((b[9]!=1) && (b[13]!=1)){
00274             speed[8]=0;
00275         }
00276         if((b[11]!=1) && (b[14]!=1)){
00277             speed[9]=0;
00278         }
00279  
00280         /*PID*/
00281         if(abs(stick[2]) < 10){
00282             if(fabs(TargetAngle-posiTheta)>8){
00283                 TargetAngle += 15*pm(posiTheta-TargetAngle);
00284                 if(abs(TargetAngle)==195){//abs(TargetAngle)==180だと永遠にこのif文に捕らわれてしまいそう。
00285                     TargetAngle += -360*pm(TargetAngle);
00286                 }
00287             }
00288             spin(TargetAngle);
00289         }
00290         /*全方位*/
00291         for(int i=0; i<4; i++){
00292             if(abs(stick[i]) > 10){
00293                 engine[i] = maxSpeed*(stick[i]/128.0);
00294             }else if(b[0]){
00295                 engine[1] = maxSpeed*(trigger[1]/225.0);
00296                 engine[0] = 0;
00297             }else if(b[1]){
00298                 engine[0] = -maxSpeed*(trigger[1]/225.0);
00299                 engine[1] = 0;
00300             }else if(b[2]){
00301                 engine[1] = -maxSpeed*(trigger[1]/225.0);
00302                 engine[0] = 0;
00303             }else if(b[3]){
00304                 engine[0] = maxSpeed*(trigger[1]/255.0);
00305                 engine[1] = 0;
00306             }else{
00307                 engine[i] = 0;
00308             }
00309         }
00310         /*旋回*/
00311         if(abs(stick[2]) > 10){
00312             spin_power = engine[2];
00313         }else{
00314             spin_power = angl.compute();
00315         }
00316         
00317         omni.computeXY(engine[0],engine[1],-spin_power);
00318         for(int i = 0; i < 4; i++) speed[i] = omni.wheel[i];
00319  
00320  
00321         for(int i=0; i<12; i++) motor[i].setSpeed(speed[i]);
00322         
00323 }
00324 void spin(double ang)
00325 {
00326     angl.setSetPoint(ang);
00327     posiTheta = posi.getTheta()*(180.0/M_PI);
00328     angl.setProcessValue(posiTheta);
00329     //for(int i=4; i<12; i++) motor[i].setSpeed(0);旋回時以外はPID判定なのでsetSpeed(0)だと機構が動かなくなるので多分このコードいらないです。
00330     //pc.printf("ang:%.2f pid:%.2f posi:%.2f T-p:%.2f\r\n",ang,-angl.compute(),posiTheta,TargetAngle-posiTheta);
00331   
00332 }
00333 int pm(double num){
00334     return((num>0)-(num<0));
00335 }