main.cpp

00001 #include "mbed.h"
00002 #include "ikarashiMDC.h"
00003 #include "controller.h"
00004 #include "pinconfig.h"
00005 #include "omni_wheel.h"
00006 #include "math.h"
00007 #include "OmniPosition.h"
00008 #include "PID.h"
00009 
00010 #define PI 3.141592653589
00011 
00012 Bcon mycon(fepTX, fepRX, fepad);
00013 Serial pc(pcTX, pcRX, 115200);
00014 Serial RS485(PC_10,PC_11,115200);
00015 
00016 
00017 uint8_t b[8];
00018 int16_t stick[4];
00019 double engine[4];
00020 double speed[6];
00021 double spin_power;
00022 double posiX , posiY , posiTheta;
00023 
00024 DigitalOut stop(A3);
00025 ikarashiMDC motor[] = {
00026     ikarashiMDC(0,0,SM,&RS485),
00027     ikarashiMDC(0,1,SM,&RS485),
00028     ikarashiMDC(0,2,SM,&RS485),
00029     ikarashiMDC(0,3,SM,&RS485),
00030 };
00031 
00032 OmniWheel omni(4);
00033 OmniPosition posi(mwTX, mwRX);
00034 
00035 PID angle(10.0, 5.0, 0.0000005, 0.01);//値はhttps://controlabo.com/pid-gain/で調整しよう！
00036 
00037 /*プロトタイプ宣言*/
00038 void chassis();//足回りの制御・ジャイロ・テラターム
00039 void spin(double ang);//PID
00040 
00041 int main(void){
00042     mycon.StartReceive();
00043 
00044     omni.wheel[0].setRadian(PI * 1.0 / 4.0);
00045     omni.wheel[1].setRadian(PI * 3.0 / 4.0);
00046     omni.wheel[2].setRadian(PI * 5.0 / 4.0);
00047     omni.wheel[3].setRadian(PI * 7.0 / 4.0);
00048     
00049     angle.setInputLimits(-180,180);
00050     angle.setOutputLimits(-0.4,0.4);
00051     angle.setBias(0);
00052     angle.setMode(1);
00053     angle.setSetPoint(0);
00054     while(1){
00055         stop = 1;
00056         chassis();
00057     }
00058     
00059 }
00060 
00061 void chassis(){
00062         /*非常停止*/
00063         //stop = 1;
00064 
00065         /*ジャイロ読み取り*/
00066         posiX = posi.getX();
00067         posiY = posi.getY();
00068         posiTheta = posi.getTheta()*(180.0/M_PI);//OmniPositionライブラリが弧度法で角度をつけていたため60分法に変換
00069         pc.printf("x:%5.2f Y:%5.2f theta:%0.3f | ",posiX, posiY,posiTheta);
00070         /*コントローラー受信*/
00071         for (int i=0; i<8; i++) b[i] = mycon.getButton(i);     
00072         for (int i=0; i<4; i++) stick[i] = mycon.getStick(i);
00073         
00074         for (int i=0; i<8; i++) pc.printf("%d ", b[i]);
00075         pc.printf(" | ");
00076         for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]);
00077         pc.printf(" | ");
00078         if (mycon.status) pc.printf("received\r\n");
00079         else pc.printf("anything error...\r\n");
00080 
00081         for(int i=0; i<4; i++){
00082             speed[i] = 0;
00083         }
00084         
00085         /*notcontoroler*/
00086         for(int i=0; i<4; i++){
00087             motor[i].setSpeed(0);
00088         }
00089         /*全方位*/
00090         for(int i=0; i<4; i++){
00091             if(abs(stick[i]) > 10){
00092             engine[i] = 0.4*(stick[i]/128.0);
00093             }else{
00094                 engine[i] = 0;
00095             }
00096         }
00097 
00098         /*旋回*/
00099         if(abs(stick[2]) > 10){
00100             spin_power = engine[2];
00101         }else{
00102             spin_power = 0;
00103         }
00104         
00105         omni.computeXY(engine[0],engine[1],-spin_power);
00106         for(int i = 0; i < 4; i++) speed[i] = omni.wheel[i];
00107 
00108 
00109         for(int i=0; i<4; i++) motor[i].setSpeed(speed[i]);
00110         /*PID*/
00111         /*スパゲッティコードで申し訳ないです...*/
00112         if(abs(stick[2]) < 10){
00113         if((fabs(posiTheta)<7.5)&&(fabs(posiTheta)>1)){
00114             spin(0);
00115         }else if(((posiTheta>=7.5)&&(posiTheta<22.5))&&((posiTheta>16)||(posiTheta<14))){
00116             spin(15);
00117         }else if(((posiTheta>=22.5)&&(posiTheta<37.5))&&((posiTheta>31)||(posiTheta<29))){
00118             spin(30);
00119         }else if(((posiTheta>=37.5)&&(posiTheta<52.5))&&((posiTheta>46)||(posiTheta<44))){
00120             spin(45);
00121         }else if(((posiTheta>=52.5)&&(posiTheta<67.5))&&((posiTheta>61)||(posiTheta<59))){
00122             spin(60);
00123         }else if(((posiTheta>=67.5)&&(posiTheta<82.5))&&((posiTheta>76)||(posiTheta<74))){
00124             spin(75);
00125         }else if(((posiTheta>=82.5)&&(posiTheta<97.5))&&((posiTheta>91)||(posiTheta<89))){
00126             spin(90);
00127         }else if(((posiTheta>=97.5)&&(posiTheta<112.5))&&((posiTheta>106)||(posiTheta<104))){
00128             spin(105);
00129         }else if(((posiTheta>=112.5)&&(posiTheta<127.5))&&((posiTheta>121)||(posiTheta<119))){
00130             spin(120);
00131         }else if(((posiTheta>=127.5)&&(posiTheta<142.5))&&((posiTheta>136)||(posiTheta<134))){
00132             spin(135);
00133         }else if(((posiTheta>=142.5)&&(posiTheta<157.5))&&((posiTheta>151)||(posiTheta<149))){
00134             spin(150);
00135         }else if(((posiTheta>=157.5)&&(posiTheta<172.5))&&((posiTheta>166)||(posiTheta<164))){
00136             spin(165);
00137         }else if(((posiTheta>=172.5)&&(posiTheta<=179))||((posiTheta<=-172.5)&&(posiTheta>=-179))){
00138             spin(180);
00139         }else if(((posiTheta<=-157.5)&&(posiTheta>-172.5))&&((posiTheta>-164)||(posiTheta<-166))){
00140             spin(-165);
00141         }else if(((posiTheta<=-142.5)&&(posiTheta>-157.5))&&((posiTheta>-149)||(posiTheta<-151))){
00142             spin(-150);
00143         }else if(((posiTheta<=-127.5)&&(posiTheta>-142.5))&&((posiTheta>-134)||(posiTheta<-136))){
00144             spin(-135);
00145         }else if(((posiTheta<=-112.5)&&(posiTheta>-127.5))&&((posiTheta>-119)||(posiTheta<-121))){
00146             spin(-120);
00147         }else if(((posiTheta<=-97.5)&&(posiTheta>-112.5))&&((posiTheta>-104)||(posiTheta<-106))){
00148             spin(-105);
00149         }else if(((posiTheta<=-82.5)&&(posiTheta>-90.5))&&((posiTheta>-89)||(posiTheta<-91))){
00150             spin(-90);
00151         }else if(((posiTheta<=-67.5)&&(posiTheta>-82.5))&&((posiTheta>-74)||(posiTheta<-76))){
00152             spin(-75);
00153         }else if(((posiTheta<=-52.5)&&(posiTheta>-67.5))&&((posiTheta>-59)||(posiTheta<-61))){
00154             spin(-60);
00155         }else if(((posiTheta<=-37.5)&&(posiTheta>-52.5))&&((posiTheta>-44)||(posiTheta<-46))){
00156             spin(-45);
00157         }else if(((posiTheta<=-22.5)&&(posiTheta>-37.5))&&((posiTheta>-29)||(posiTheta<-30))){
00158             spin(-30);
00159         }else if(((posiTheta<=-7.5)&&(posiTheta>-22.5))&&((posiTheta>-14)||(posiTheta<-16))){
00160             spin(-15);
00161         }else{
00162         }
00163         }
00164 }
00165 void spin(double ang)
00166 {
00167     angle.setSetPoint(ang);
00168         while(1) {
00169         stop = 1;
00170         posiTheta = posi.getTheta()*(180.0/M_PI);
00171         angle.setProcessValue(posiTheta);
00172         pc.printf("spinning... |%d|%.2f|%.3f\r\n",ang,posiTheta,-angle.compute());
00173         for(int i=0; i<4; i++) motor[i].setSpeed(-angle.compute());
00174         //for(int i=4; i<8; i++) motor[i].setSpeed(0);射出機構とかのモーターが出てきたときに[//]を消す
00175         if ((fabs(ang - posiTheta) < 1)||(fabs(ang - posiTheta)> 7.5)) return;
00176     }
00177 }