春ロボ1班(元F3RC4班+) / Mbed 2 deprecated harurobo1006

とりあえず 簡単に

Dependencies:   CruizCore_R1370P EC_speedcontrol_1010 enc_1multi mbed

main.cpp@1:2189d6238ac7, 2018-10-26 (annotated)

Committer:
aoikoizumi
Date:
Fri Oct 26 05:13:04 2018 +0000
Revision:
1:2189d6238ac7
Parent:
0:b811f40948e2 
10/26 ???????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aoikoizumi 0:b811f40948e2 1 #include "mbed.h"
aoikoizumi 1:2189d6238ac7 2 #include "SpeedController.h"//最終的にはSpeedController使わないけど
aoikoizumi 1:2189d6238ac7 3 #include "EC.h"//エンコーダ
aoikoizumi 1:2189d6238ac7 4 #include "R1370P.h"//ジャイロ
aoikoizumi 1:2189d6238ac7 5 //#include "enc_1multi.h"
aoikoizumi 0:b811f40948e2 6 #include "math.h"
aoikoizumi 1:2189d6238ac7 7 #define BASIC_SPEED 24 //モーターはこの角速度で駆動させる//SpeedController
aoikoizumi 0:b811f40948e2 8 #define PI 3.141592
aoikoizumi 0:b811f40948e2 9
aoikoizumi 0:b811f40948e2 10
aoikoizumi 0:b811f40948e2 11 SpeedControl Motor_RF(NC,NC,NC,500,0.05,NC,NC);
aoikoizumi 0:b811f40948e2 12 SpeedControl Motor_LF(NC,NC,NC,500,0.05,NC,NC);
aoikoizumi 0:b811f40948e2 13 SpeedControl Motor_LB(NC,NC,NC,500,0.05,NC,NC);
aoikoizumi 1:2189d6238ac7 14 SpeedControl Motor_RB(NC,NC,NC,500,0.05,NC,NC);//モーター定義//SpeedController
aoikoizumi 0:b811f40948e2 15
aoikoizumi 0:b811f40948e2 16 Ec EC1(NC,NC,NC,300,0.05);
aoikoizumi 0:b811f40948e2 17 Ec EC2(NC,NC,NC,300,0.05);//エンコーダ定義
aoikoizumi 0:b811f40948e2 18
aoikoizumi 0:b811f40948e2 19 Ticker motor_tick; //角速度計算用ticker
aoikoizumi 0:b811f40948e2 20 Ticker ticker;//for enc
aoikoizumi 0:b811f40948e2 21
aoikoizumi 0:b811f40948e2 22 Serial pc(USBTX, USBRX); // tx, rx //PC USB
aoikoizumi 0:b811f40948e2 23 R1370P gyro(PC_6,PC_7);
aoikoizumi 0:b811f40948e2 24
aoikoizumi 0:b811f40948e2 25 void calOmega() //角速度計算関数
aoikoizumi 0:b811f40948e2 26 {
aoikoizumi 0:b811f40948e2 27 Motor_RF.CalOmega();
aoikoizumi 0:b811f40948e2 28 Motor_LF.CalOmega();
aoikoizumi 0:b811f40948e2 29 Motor_LB.CalOmega();
aoikoizumi 1:2189d6238ac7 30 Motor_RB.CalOmega();//SpeedController
aoikoizumi 0:b811f40948e2 31 EC1.CalOmega();
aoikoizumi 0:b811f40948e2 32 EC2.CalOmega();
aoikoizumi 0:b811f40948e2 33 }
aoikoizumi 0:b811f40948e2 34 double new_dist1=0,new_dist2=0;
aoikoizumi 0:b811f40948e2 35 double old_dist1=0,old_dist2=0;
aoikoizumi 0:b811f40948e2 36 double d_dist1=0,d_dist2=0;//座標計算用関数
aoikoizumi 0:b811f40948e2 37 double now_x;//現在地X座標
aoikoizumi 0:b811f40948e2 38 double now_y;//現在地Y座標
aoikoizumi 0:b811f40948e2 39 double now_angle; //現在角度
aoikoizumi 0:b811f40948e2 40
aoikoizumi 0:b811f40948e2 41 double start_x=0;
aoikoizumi 1:2189d6238ac7 42 double start_y=0;//起点
aoikoizumi 0:b811f40948e2 43
aoikoizumi 0:b811f40948e2 44 double target_RF=0,target_LF=0,target_LB=0,target_RB=0;//目標速度
aoikoizumi 0:b811f40948e2 45 Timer t;
aoikoizumi 0:b811f40948e2 46
aoikoizumi 0:b811f40948e2 47
aoikoizumi 0:b811f40948e2 48
aoikoizumi 1:2189d6238ac7 49
aoikoizumi 1:2189d6238ac7 50 void output(double iRF,double iLF,double iLB,double iRB)//後にSPI用に変える
aoikoizumi 0:b811f40948e2 51 {
aoikoizumi 0:b811f40948e2 52 target_RF=BASIC_SPEED*iRF;
aoikoizumi 0:b811f40948e2 53 target_LF=BASIC_SPEED*iLF;
aoikoizumi 0:b811f40948e2 54 target_LB=BASIC_SPEED*iLB;
aoikoizumi 0:b811f40948e2 55 target_RB=BASIC_SPEED*iRB;
aoikoizumi 0:b811f40948e2 56 }
aoikoizumi 1:2189d6238ac7 57 void base(double bRF,double bLF,double bLB,double bRB,double Max)//いろんな加算をしても最大OR最小が１になるような補正(?)//絶対値が一番でかいやつで除算//double Max(0~1)
aoikoizumi 1:2189d6238ac7 58 {
aoikoizumi 1:2189d6238ac7 59
aoikoizumi 1:2189d6238ac7 60 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));
aoikoizumi 1:2189d6238ac7 61 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));
aoikoizumi 1:2189d6238ac7 62 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));
aoikoizumi 1:2189d6238ac7 63 else output(Max*bRF/fabs(bRB),Max*bLF/fabs(bRB),Max*bLB/fabs(bRB),Max );
aoikoizumi 1:2189d6238ac7 64 }
aoikoizumi 0:b811f40948e2 65
aoikoizumi 0:b811f40948e2 66
aoikoizumi 1:2189d6238ac7 67
aoikoizumi 1:2189d6238ac7 68 //ここからそれぞれのプログラム//////////////////////////////////////////////////////
aoikoizumi 1:2189d6238ac7 69 //now_x(現在のx座標),now_y(現在のy座標),now_angle(機体角度(ラジアンではない)(0~360や-180~180とは限らない))(反時計回りが正)
aoikoizumi 1:2189d6238ac7 70 //ジャイロの出力は角度だが三角関数はラジアンとして計算する
aoikoizumi 1:2189d6238ac7 71 //通常の移動＋座標のずれ補正＋機体の角度補正(＋必要に応じさらに別補正)
aoikoizumi 1:2189d6238ac7 72 //ジャイロの仕様上、角度補正をするときに計算式内で角度はそのままよりsinをとったほうがいいかもね
aoikoizumi 1:2189d6238ac7 73
aoikoizumi 1:2189d6238ac7 74 void gostraight(int type,double goal_x,double goal_y,double speed,double front)//移動パターン(1,2,3,4)、目標X、目標Y、最高速度(0~1)、正面角度
aoikoizumi 1:2189d6238ac7 75 {
aoikoizumi 1:2189d6238ac7 76 double y_hosei=(now_y-goal_y)*0.001;//Y座標(mm単位)にP処理
aoikoizumi 1:2189d6238ac7 77 double x_hosei=(now_x-goal_x)*0.001;//X座標(mm単位)にP処理
aoikoizumi 1:2189d6238ac7 78 double incl_hosei=sin(now_angle-front)*(PI/180)*0.1;//機体角度(sin(数度→ラジアンに変換))にP処理
aoikoizumi 1:2189d6238ac7 79
aoikoizumi 1:2189d6238ac7 80 switch(type) {
aoikoizumi 1:2189d6238ac7 81 case 1://Y座標一定の正方向横移動
aoikoizumi 1:2189d6238ac7 82 while(now_x<goal_x){
aoikoizumi 1:2189d6238ac7 83 base(-1-y_hosei-incl_hosei,-1+y_hosei-incl_hosei,1+y_hosei-incl_hosei,1-y_hosei-incl_hosei,speed);
aoikoizumi 1:2189d6238ac7 84 }
aoikoizumi 1:2189d6238ac7 85 break;
aoikoizumi 1:2189d6238ac7 86
aoikoizumi 1:2189d6238ac7 87 case 2://Y座標一定の負方向横移動
aoikoizumi 1:2189d6238ac7 88 while(now_x>goal_x){
aoikoizumi 1:2189d6238ac7 89 base(1-y_hosei-incl_hosei,1+y_hosei-incl_hosei,-1+y_hosei-incl_hosei,-1-y_hosei-incl_hosei,speed);
aoikoizumi 1:2189d6238ac7 90 }
aoikoizumi 1:2189d6238ac7 91 break;
aoikoizumi 1:2189d6238ac7 92
aoikoizumi 1:2189d6238ac7 93 case 3://Y座標一定の正方向横移動
aoikoizumi 1:2189d6238ac7 94 while(now_y<goal_y){
aoikoizumi 1:2189d6238ac7 95 base(1+x_hosei-incl_hosei,-1+x_hosei-incl_hosei,-1-x_hosei-incl_hosei,1-x_hosei-incl_hosei,speed);
aoikoizumi 1:2189d6238ac7 96 }
aoikoizumi 1:2189d6238ac7 97 break;
aoikoizumi 1:2189d6238ac7 98
aoikoizumi 1:2189d6238ac7 99 case 4://X座標一定の負方向横移動
aoikoizumi 1:2189d6238ac7 100 while(now_y>goal_y){
aoikoizumi 1:2189d6238ac7 101 base(-1+x_hosei-incl_hosei,1+x_hosei-incl_hosei,1-x_hosei-incl_hosei,-1-x_hosei-incl_hosei,speed);
aoikoizumi 1:2189d6238ac7 102 }
aoikoizumi 1:2189d6238ac7 103 break;
aoikoizumi 1:2189d6238ac7 104 }
aoikoizumi 1:2189d6238ac7 105 }
aoikoizumi 1:2189d6238ac7 106
aoikoizumi 1:2189d6238ac7 107 //ここまで///////////////////////////////////////////////////////////////////////
aoikoizumi 1:2189d6238ac7 108
aoikoizumi 0:b811f40948e2 109 int main()
aoikoizumi 0:b811f40948e2 110 {
aoikoizumi 0:b811f40948e2 111 gyro.initialize(); //main関数の最初に一度だけ実行
aoikoizumi 0:b811f40948e2 112 gyro.acc_offset(); //やってもやらなくてもいい
aoikoizumi 1:2189d6238ac7 113 //printf("start\r\n");
aoikoizumi 0:b811f40948e2 114
aoikoizumi 0:b811f40948e2 115 motor_tick.attach(&calOmega,0.05);
aoikoizumi 0:b811f40948e2 116
aoikoizumi 0:b811f40948e2 117 EC1.setDiameter_mm(48);
aoikoizumi 0:b811f40948e2 118 EC2.setDiameter_mm(48);//測定輪半径
aoikoizumi 0:b811f40948e2 119 double getDistance_mm();
aoikoizumi 0:b811f40948e2 120
aoikoizumi 0:b811f40948e2 121 void reset();
aoikoizumi 0:b811f40948e2 122 EC1.reset();
aoikoizumi 0:b811f40948e2 123
aoikoizumi 0:b811f40948e2 124 now_x=start_x;
aoikoizumi 0:b811f40948e2 125 now_y=start_y;
aoikoizumi 1:2189d6238ac7 126
aoikoizumi 0:b811f40948e2 127 while(1) {
aoikoizumi 0:b811f40948e2 128 now_angle=gyro.getAngle();
aoikoizumi 0:b811f40948e2 129
aoikoizumi 0:b811f40948e2 130 new_dist1=EC1.getDistance_mm();
aoikoizumi 0:b811f40948e2 131 new_dist2=EC2.getDistance_mm();
aoikoizumi 0:b811f40948e2 132 d_dist1=new_dist1-old_dist1;
aoikoizumi 0:b811f40948e2 133 d_dist2=new_dist2-old_dist2;
aoikoizumi 0:b811f40948e2 134 old_dist1=new_dist1;
aoikoizumi 0:b811f40948e2 135 old_dist2=new_dist2;//微小時間当たりのエンコーダ読み込み
aoikoizumi 0:b811f40948e2 136
aoikoizumi 0:b811f40948e2 137
aoikoizumi 1:2189d6238ac7 138 double d_x=d_dist2*sin(now_angle*PI/180)-d_dist1*sin(now_angle*PI/180);
aoikoizumi 1:2189d6238ac7 139 double d_y=d_dist2*sin(now_angle*PI/180)+d_dist1*cos(now_angle*PI/180);//微小時間毎の座標変化
aoikoizumi 0:b811f40948e2 140
aoikoizumi 0:b811f40948e2 141
aoikoizumi 0:b811f40948e2 142 now_x=now_x+d_x;
aoikoizumi 0:b811f40948e2 143 now_y=now_y+d_y;//微小時間毎に座標に加算
aoikoizumi 0:b811f40948e2 144 }
aoikoizumi 0:b811f40948e2 145 }