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 
00049 ////abe 足回り
00050 //double engine[4];
00051 //double spin_power;
00052 //double posiX , posiY , posiTheta;
00053 //int TargetAngle = 0;
00054 //int StartAngle = 0;
00055 
00056 //OmniWheel omni(4);
00057 //OmniPosition posi(sub1TX, sub1RX);
00058 //
00059 //PID angle(10.0, 5.0, 0.0000005, 0.01);
00060 
00061 ////プロトタイプ宣言
00062 //void chassis();         //足回りの制御・ジャイロ・テラターム
00063 //void spin(double ang);  //PID
00064 //int  pm(double num);    //正負判定
00065 
00066 Timer t;
00067 
00068 int16_t trigger[4];
00069 uint8_t b[16];
00070 int16_t stick[4];
00071 uint8_t data[128];
00072 int pw;
00073 
00074 double speed[12] = {0};
00075 
00076 int main() 
00077 {
00078     t.start();
00079 
00080     int16_t volume[3];
00081     uint8_t toggle[4];
00082     uint8_t timeout;
00083     
00084     double bldcspeed = 0.6;
00085     
00086     rcv.setHeaders(0xff,0xff);
00087     rcv.startReceive(4); //SerialMultiByte受信
00088     
00089     mycon.StartReceive(); //コントローラー受信・宣言
00090     
00091 　  
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 //    angle.setInputLimits(-180,180);
00099 //    angle.setOutputLimits(-0.4,0.4);
00100 //    angle.setBias(0);
00101 //    angle.setMode(1);
00102 //    angle.setSetPoint(0);
00103 
00104     
00105     int16_t angle[4] = {0};//エンコーダ受信格納
00106     uint8_t pulse[8] = {0};
00107     
00108     while(1)
00109         {
00110         stop = toggle[0];
00111         
00112         mycon.getData(data);//コントローラー受信
00113         for (int i=0, tmp=1; i<8; i++) {
00114             pw = pow((float)2,i);
00115             b[i] = (int)((data[0] & tmp)/pw);
00116 //            pc.printf("%d ", b[i]);
00117             tmp *= 2;
00118         }
00119         for (int i=8, tmp=1, j=0; i<16; i++, j++) {
00120             pw = pow((float)2,j);
00121             b[i] = (int)((data[1] & tmp)/pw);
00122 //            pc.printf("%d ", b[i]);
00123             tmp *= 2;
00124         }
00125 //        pc.printf(" | ");
00126         for (int i=0; i<4; i++) {
00127             stick[i] = data[i+2];
00128 //            pc.printf("%3d ", stick[i]);
00129         }
00130 //        pc.printf(" | ");
00131         for (int i=0; i<2; i++) {
00132             trigger[i] = data[i+6];
00133 //            pc.printf("%3d ", trigger[i]);
00134         }
00135 //        pc.printf(" | ");
00136         for (int i=0; i<3; i++) {
00137             volume[i] = data[i+9];
00138 //            pc.printf("%3d ", volume[i]);
00139         }
00140         pc.printf(" | ");
00141         for (int i=0; i<4; i++) {
00142             toggle[i] = data[i+12];
00143 //            pc.printf("%3d ", toggle[i]);
00144         }
00145 //        pc.printf(" | ");
00146         timeout = data[8];
00147         pc.printf("%3d ", timeout);
00148 //        pc.printf(" | ");
00149         if (mycon.getStatus()) pc.printf("receive");
00150         else{pc.printf("anything error..."); 
00151                 for( int i=0; i<12; i++){
00152                 motor[i].setSpeed(0);
00153             }
00154         }
00155         
00156         
00157 //        /*阿部君の素晴らしい天才的な足回り関数*/
00158 //        chassis();
00159         
00160         rcv.getData(pulse); //エンコーダ受信
00161         for(int i=0,j=0;i<4;i++,j+=2){
00162             angle[i] = pulse[j] << 8 | pulse[j+1];  
00163         }
00164         pc.printf("| ENC1:%d | ENC2:%d | ENC3:%d | ENC4:%d |",angle[0],angle[1],angle[2],angle[3]);            
00165         pc.printf("\r\n");
00166 
00167         //ブラシレスモーター
00168         pwm_imput1 = ((double)volume[0]/255.0)*bldcspeed*toggle[1];
00169 
00170         pwm_imput2 = ((double)volume[1]/255.0)*bldcspeed*toggle[2];
00171 
00172         pwm_imput3 = ((double)volume[2]/255.0)*bldcspeed*toggle[3];
00173 //        pc.printf("servo:%.2f | servo2:%.2f | servo3:%.2f\r\n",
00174 //                  ((double)volume[0]/255.0)*bldcspeed,((double)volume[1]/255.0)*bldcspeed,((double)volume[2]/255.0)*bldcspeed);
00175         
00176         /*井上機構ON,OFF*/
00177         if(toggle[1]){
00178             speed[10] = -0.9;
00179         }else{
00180             speed[10] = 0;
00181         }
00182         if(toggle[1] && b[15]){
00183             speed[10] = 0.4;
00184         }
00185         
00186         if(toggle[3]){
00187             speed[11] = -0.9;
00188         }else{
00189             speed[11] = 0;
00190         }
00191         if(toggle[3] && b[15]){
00192             speed[11] = 0.4;
00193         }
00194         
00195         /*フジモン機構*/
00196         if(toggle[2]){
00197             speed[6] = 0.6;
00198             speed[7] = 0.6;   
00199         }else{
00200             speed[6] = 0;
00201             speed[7] = 0;            
00202         }
00203         
00204         /*展開昇降*/
00205         if(b[5] != 0){
00206             speed[4] = 0.5;
00207             speed[5] = 0.5;
00208         }else if(b[4] != 0){
00209             speed[4] = -0.35;
00210             speed[5] = -0.35;
00211         }else{
00212             speed[4] = 0;
00213             speed[5] = 0;
00214         }
00215         //機構昇降
00216         if(b[9]){
00217             speed[8] = -0.32;
00218         }else
00219         if(b[13]){    
00220             speed[8] = 0.4;
00221         }
00222         if(b[11]){
00223             speed[9] = -0.32;
00224         }else
00225         if(b[14]){    
00226             speed[9] = 0.4;
00227         }
00228         if((b[9]!=1) && (b[13]!=1)){
00229             speed[8]=0;
00230         }
00231         if((b[11]!=1) && (b[14]!=1)){
00232             speed[9]=0;
00233         }
00234         for(int i=0; i<12; i++) pc.printf("%.2f ",speed[i]);
00235         for(int i=4; i<12; i++) motor[i].setSpeed(speed[i]);
00236     }
00237         
00238 }        
00239 
00240 //void chassis(){
00241 //    
00242 //        mycon.getData(data);
00243 //        for (int i=0; i<4; i++) {
00244 //            stick[i] = data[i+2];
00245 //        }
00246 //
00247 //        /*ジャイロ読み取り*/
00248 //        posiX = posi.getX();
00249 //        posiY = posi.getY();
00250 //        posiTheta = posi.getTheta()*(180.0/M_PI);//OmniPositionライブラリが弧度法で角度をつけていたため60分法に変換
00251 //        pc.printf("x:%5.2f Y:%5.2f theta:%0.3f | ",posiX, posiY,posiTheta);
00252 //
00253 //        if(abs(stick[2]) < 10){
00254 //            if(fabs(TargetAngle-posiTheta)>8){
00255 //                TargetAngle += 15*pm(posiTheta-TargetAngle);
00256 //                if(abs(TargetAngle)==195){//abs(TargetAngle)==180だと永遠にこのif文に捕らわれてしまいそう。
00257 //                    TargetAngle += -360*pm(TargetAngle);
00258 //                }
00259 //            }
00260 //            spin(TargetAngle);
00261 //        }
00262 //
00263 //        /*全方位*/
00264 //        for(int i=0; i<4; i++){
00265 //            if(abs(stick[i]) > 10){
00266 //                if(trigger[1]<10) trigger[1] = 0;
00267 //                engine[i] = maxSpeed*(stick[i]/128.0)*(trigger[1]/255.0);
00268 //            }else if(b[i]%2){
00269 //                if(trigger[1]<10) trigger[1] = 0;
00270 //                //b[1]のとき左向き（負）b[3]のとき右向き（正）
00271 //                engine[0] = maxSpeed*pm(i-2)*(trigger[1]/255.0);
00272 //                engine[1] = 0;
00273 //            }else if(b[i]%2==0){
00274 //                if(trigger[1]<10) trigger[1] = 0;
00275 //                //b[0]のとき上向き（正）b[2]のとき下向き（負）
00276 //                engine[1] = -maxSpeed*pm(i-1)*(trigger[1]/255.0);
00277 //                engine[0] = 0;
00278 //            }else{
00279 //                engine[i] = 0;
00280 //                }
00281 //        }
00282 //        /*旋回*/
00283 //        if(abs(stick[2]) > 10){
00284 //            spin_power = engine[2];
00285 //        }else{
00286 //            spin_power = angle.compute();
00287 //        }
00288 //        
00289 //        omni.computeXY(engine[0],engine[1],-spin_power);
00290 //        for(int i = 0; i < 4; i++) speed[i] = omni.wheel[i];
00291 //        for(int i=0; i<4; i++) motor[i].setSpeed(speed[i]);
00292 //        
00293 //}
00294 //
00295 //
00296 //void spin(double ang)
00297 //{
00298 //    angle.setSetPoint(ang);
00299 //    posiTheta = posi.getTheta()*(180.0/M_PI);
00300 //    angle.setProcessValue(posiTheta);
00301 //    pc.printf("ang:%.2f pid:%.2f    posi:%.2f T-p:%.2f\r\n",ang,-angle.compute(),posiTheta,TargetAngle-posiTheta);
00302 //    //for(int i=4; i<12; i++) motor[i].setSpeed(0);射出機構とかのモーターが出てきたときに[//]を消す    
00303 //}
00304 //
00305 //
00306 //int pm(double num){
00307 //    return((num>0)-(num<0));
00308 //}