main.cpp

00001 #include "mbed.h"
00002 #include "SpeedController.h"//最終的にはSpeedController使わないけど
00003 #include "EC.h"//エンコーダ
00004 #include "R1370P.h"//ジャイロ
00005 //#include "enc_1multi.h"
00006 #include "math.h"
00007 #define BASIC_SPEED 24  //モーターはこの角速度で駆動させる//SpeedController
00008 #define PI 3.141592
00009 
00010 
00011 SpeedControl Motor_RF(NC,NC,NC,500,0.05,NC,NC);
00012 SpeedControl Motor_LF(NC,NC,NC,500,0.05,NC,NC);
00013 SpeedControl Motor_LB(NC,NC,NC,500,0.05,NC,NC);
00014 SpeedControl Motor_RB(NC,NC,NC,500,0.05,NC,NC);//モーター定義//SpeedController
00015 
00016 Ec EC1(NC,NC,NC,300,0.05);
00017 Ec EC2(NC,NC,NC,300,0.05);//エンコーダ定義
00018 
00019 Ticker motor_tick;  //角速度計算用ticker
00020 Ticker ticker;//for enc
00021 
00022 Serial pc(USBTX, USBRX); // tx, rx //PC USB
00023 R1370P gyro(PC_6,PC_7);
00024 
00025 void calOmega() //角速度計算関数
00026 {
00027     Motor_RF.CalOmega();
00028     Motor_LF.CalOmega();
00029     Motor_LB.CalOmega();
00030     Motor_RB.CalOmega();//SpeedController
00031     EC1.CalOmega();
00032     EC2.CalOmega();
00033 }
00034 double new_dist1=0,new_dist2=0;
00035 double old_dist1=0,old_dist2=0;
00036 double d_dist1=0,d_dist2=0;//座標計算用関数
00037 double now_x;//現在地X座標
00038 double now_y;//現在地Y座標
00039 double now_angle; //現在角度
00040 
00041 double start_x=0;
00042 double start_y=0;//起点
00043 
00044 double target_RF=0,target_LF=0,target_LB=0,target_RB=0;//目標速度
00045 Timer t;
00046 
00047 
00048 
00049 
00050 void output(double iRF,double iLF,double iLB,double iRB)//後にSPI用に変える
00051 {
00052     target_RF=BASIC_SPEED*iRF;
00053     target_LF=BASIC_SPEED*iLF;
00054     target_LB=BASIC_SPEED*iLB;
00055     target_RB=BASIC_SPEED*iRB;
00056 }
00057 void base(double bRF,double bLF,double bLB,double bRB,double Max)//いろんな加算をしても最大OR最小が１になるような補正(?)//絶対値が一番でかいやつで除算//double Max(0~1)
00058 {
00059     
00060     if     (fabs(bRF)>=fabs(bLF)&&fabs(bRF)>=fabs(bLB)&&fabs(bRF)>=fabs(bRB))output(Max              ,Max*bLF/fabs(bRF),Max*bLB/fabs(bRF),Max*bRB/fabs(bRF));
00061     else if(fabs(bLF)>=fabs(bRF)&&fabs(bLF)>=fabs(bLB)&&fabs(bLF)>=fabs(bRB))output(Max*bRF/fabs(bLF),Max              ,Max*bLB/fabs(bLF),Max*bRB/fabs(bLF));
00062     else if(fabs(bLB)>=fabs(bRF)&&fabs(bLB)>=fabs(bLF)&&fabs(bLB)>=fabs(bRB))output(Max*bRF/fabs(bLB),Max*bLF/fabs(bLB),Max              ,Max*bRB/fabs(bLB));
00063     else                                                                     output(Max*bRF/fabs(bRB),Max*bLF/fabs(bRB),Max*bLB/fabs(bRB),Max              );
00064 }
00065 
00066 
00067 
00068 //ここからそれぞれのプログラム//////////////////////////////////////////////////////
00069 //now_x(現在のx座標),now_y(現在のy座標),now_angle(機体角度(ラジアンではない)(0~360や-180~180とは限らない))(反時計回りが正)
00070 //ジャイロの出力は角度だが三角関数はラジアンとして計算する
00071 //通常の移動＋座標のずれ補正＋機体の角度補正(＋必要に応じさらに別補正)
00072 //ジャイロの仕様上、角度補正をするときに計算式内で角度はそのままよりsinをとったほうがいいかもね
00073 
00074 void gostraight(int type,double goal_x,double goal_y,double speed,double front)//移動パターン(1,2,3,4)、目標X、目標Y、最高速度(0~1)、正面角度
00075 {
00076     double y_hosei=(now_y-goal_y)*0.001;//Y座標(mm単位)にP処理
00077     double x_hosei=(now_x-goal_x)*0.001;//X座標(mm単位)にP処理
00078     double incl_hosei=sin(now_angle-front)*(PI/180)*0.1;//機体角度(sin(数度→ラジアンに変換))にP処理
00079 
00080     switch(type) {
00081         case 1://Y座標一定の正方向横移動
00082             while(now_x<goal_x){
00083             base(-1-y_hosei-incl_hosei,-1+y_hosei-incl_hosei,1+y_hosei-incl_hosei,1-y_hosei-incl_hosei,speed);
00084             }
00085             break;
00086             
00087         case 2://Y座標一定の負方向横移動
00088             while(now_x>goal_x){
00089             base(1-y_hosei-incl_hosei,1+y_hosei-incl_hosei,-1+y_hosei-incl_hosei,-1-y_hosei-incl_hosei,speed);
00090             }
00091             break;
00092             
00093         case 3://Y座標一定の正方向横移動
00094             while(now_y<goal_y){
00095             base(1+x_hosei-incl_hosei,-1+x_hosei-incl_hosei,-1-x_hosei-incl_hosei,1-x_hosei-incl_hosei,speed);
00096             }
00097             break;
00098             
00099         case 4://X座標一定の負方向横移動
00100             while(now_y>goal_y){
00101             base(-1+x_hosei-incl_hosei,1+x_hosei-incl_hosei,1-x_hosei-incl_hosei,-1-x_hosei-incl_hosei,speed);
00102             }
00103             break;
00104     }
00105 }
00106 
00107 //ここまで///////////////////////////////////////////////////////////////////////
00108 
00109 int main()
00110 {
00111     gyro.initialize();    //main関数の最初に一度だけ実行
00112     gyro.acc_offset();    //やってもやらなくてもいい
00113     //printf("start\r\n");
00114 
00115     motor_tick.attach(&calOmega,0.05);
00116 
00117     EC1.setDiameter_mm(48);
00118     EC2.setDiameter_mm(48);//測定輪半径
00119     double  getDistance_mm();
00120 
00121     void reset();
00122     EC1.reset();
00123 
00124     now_x=start_x;
00125     now_y=start_y;
00126 
00127     while(1) {
00128         now_angle=gyro.getAngle();
00129 
00130         new_dist1=EC1.getDistance_mm();
00131         new_dist2=EC2.getDistance_mm();
00132         d_dist1=new_dist1-old_dist1;
00133         d_dist2=new_dist2-old_dist2;
00134         old_dist1=new_dist1;
00135         old_dist2=new_dist2;//微小時間当たりのエンコーダ読み込み
00136 
00137 
00138         double d_x=d_dist2*sin(now_angle*PI/180)-d_dist1*sin(now_angle*PI/180);
00139         double d_y=d_dist2*sin(now_angle*PI/180)+d_dist1*cos(now_angle*PI/180);//微小時間毎の座標変化
00140 
00141 
00142         now_x=now_x+d_x;
00143         now_y=now_y+d_y;//微小時間毎に座標に加算
00144     }
00145 }