main.cpp

00001 #include "mbed.h"
00002 #include "FEP_RX22.h"
00003 #include "pinconfig.h"
00004 #include "ikarashiMDC.h"
00005 #include "QEI.h"
00006 #include "PID.h"
00007 #include "can_tr.h"
00008 #include "SEKIkikou.h"
00009 //#include "HOSOKIkikou.h"
00010 
00011 FEP_RX22 mycon(fepTX, fepRX, fepad);
00012 Serial pc(USBTX, USBRX, 115200);
00013 Serial serial(motorTX, motorRX, 115200);
00014 //QEI enc1(encoder1_A, encoder1_B, NC, 100, QEI::X4_ENCODING);
00015 QEI enc2(encoder2_A, encoder2_B, NC, 100, QEI::X4_ENCODING);
00016 QEI enc3(encoder3_A, encoder3_B, NC, 100, QEI::X4_ENCODING);
00017 QEI enc4(encoder4_A, encoder4_B, NC, 100, QEI::X4_ENCODING);
00018 PID front(2.0, 0, 0.001, 0.01);
00019 DigitalOut stop(stop_pin);
00020 DigitalOut led(LED2);
00021 
00022 ikarashiMDC motor[] = {
00023     ikarashiMDC(0,0,SM,&serial),
00024     ikarashiMDC(0,1,SM,&serial),
00025     ikarashiMDC(0,2,SM,&serial),
00026     ikarashiMDC(0,3,SM,&serial),
00027     ikarashiMDC(1,0,SM,&serial),
00028     ikarashiMDC(1,1,SM,&serial),
00029     ikarashiMDC(1,2,SM,&serial),
00030     ikarashiMDC(1,3,SM,&serial)
00031 };
00032 
00033 uint8_t data[128];
00034 double frontAngle=0;
00035 
00036 void recieveController(){
00037     mycon.getData(data);
00038     for (int i=7,j=128; j>0; i--,j/=2){
00039         b[i]=data[0]/j;
00040         data[0]%=j;
00041     }
00042 //    for (int i=0; i<8; i++) pc.printf("%d ", b[i]);
00043     for (int i=0; i<4; i++) stick[i]=128-data[i+1];
00044     if(mycon.getStatus()==false){
00045         for (int i=0; i<8; i++) b[i]=0;
00046         for (int i=0; i<4; i++) stick[i]=0;
00047     }
00048     /*
00049     for (int i=0; i<4; i++) pc.printf("%d ", stick[i]);
00050     pc.printf(" | ");
00051     if (mycon.getStatus()) pc.printf("received\r\n");
00052     else pc.printf("anything error...\r\n");*/
00053 }
00054 
00055 void updateenc(){
00056 //    encoderValue[0] = (float)enc1.getPulses();
00057     encoderValue[4] = (float)enc2.getPulses();
00058     encoderValue[5] = (float)enc3.getPulses();
00059     encoderValue[6] = (float)enc4.getPulses();
00060 //    for (int i=0; i<4; i++) pc.printf("%d  ", encoderValue[i]);
00061 //    pc.printf("\r\n ");
00062 }
00063 
00064 void allanglemove(double one,double two,double three,double four){
00065     if(stick[0]>20||stick[0]<-20||stick[1]>20||stick[1]<-20){//全方位移動
00066         for(int i=0; i < 4; i++){
00067             motorSpeed[i+8] = sin((atan2((double)stick[0],(double)stick[1])-3.14*(i*2-1) / 4)+(frontAngle*3.14/180));
00068         }
00069     } else {
00070         motorSpeed[8]=one;
00071         motorSpeed[9]=two;
00072         motorSpeed[10]=three;
00073         motorSpeed[11]=four;
00074     }
00075 }
00076 
00077 void PIDset(){
00078     front.setInputLimits (-180,180);
00079     front.setOutputLimits (-1,1);
00080     front.setBias(0);
00081     front.setMode (1);
00082     front.setSetPoint(0);
00083 }
00084 
00085 void addPID(){
00086     front.setProcessValue (frontAngle);
00087     for(int i=8; i < 12; i++){
00088         motorSpeed[i]-=front.compute();
00089     }
00090 }
00091 
00092 int main()
00093 {
00094     mycon.StartReceive();
00095     recieveController();
00096     stop = 1;
00097     led = 1;
00098     
00099     printf("success!\r\n");
00100     
00101     for(int i=0; i < 8; i++){
00102         motor[i].setSpeed(0);
00103     }
00104     
00105 //    canAllReset();   
00106     PIDset();
00107     SEKIkikou seki(&motor[6], &motor[4], &motor[5], &b[0], &b[1], &b[2], &b[3], &encoderValue[4], &encoderValue[5], &encoderValue[6]);
00108     
00109     while(1) {
00110         recieveController();
00111         canTR();
00112         updateenc();
00113         frontAngle=jyroValue;
00114         
00115         allanglemove(0,0,0,0);
00116         addPID();
00117         
00118         if(b[5]&&b[6]){
00119             motorSpeed[15]=0;
00120         }else if(b[5]){
00121             motorSpeed[15]=0.5;
00122         }else if(b[6]){
00123             motorSpeed[15]=-0.5;
00124         }else{
00125             motorSpeed[15]=0;
00126         }
00127         
00128         //関機構
00129         
00130         if(b[7]==1){
00131             seki.runAll(0.1,0.3,-0.2);
00132         }else{
00133             seki.runAll(-0.8,0.3,0.3);
00134         }
00135         
00136         //モータースピード設定
00137         
00138         for(int i=0; i < 4; i++){
00139             motor[i].setSpeed(motorSpeed[i+8]*0.8);
00140         }
00141         
00142         motor[7].setSpeed(motorSpeed[15]);
00143         
00144         //以下デバッグ用pc.printf
00145         
00146         pc.printf("|wheel:  ");
00147         for(int i=8; i < 12; i++){
00148             pc.printf("%.2f  ",motorSpeed[i]);
00149         }
00150         pc.printf("|spin:  %.2f  ",motorSpeed[15]);
00151 //        pc.printf("|updown:  %.2f  ",motorSpeed[2]);
00152         pc.printf("|enc:  ");
00153         for(int i=0; i < 7; i++){
00154             pc.printf("%d  ",encoderValue[i]);
00155             
00156         }
00157         pc.printf("|  ");
00158         pc.printf("R1370:%.3f",jyroValue);
00159         pc.printf("|  ");
00160         for(int i=0; i < 8; i++){
00161             pc.printf("%d ",b[i]);
00162         }
00163         pc.printf("|  ");
00164         for(int i=0; i < 4; i++){
00165             pc.printf("%d  ",stick[i]);
00166         }
00167         pc.printf("\r\n");
00168         
00169     }
00170 }