春ロボ ロケット団
大会本番用
Dependencies: mbed SpeedController hcsr04 Encoder CruizCore_R1370P
Diff: main.cpp
- Revision:
- 0:c0e9bbc27454
- Child:
- 1:a692014d8e41
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Mar 11 05:54:39 2020 +0000
@@ -0,0 +1,296 @@
+#include "mbed.h"
+#include "EC.h"
+#include "SpeedController.h"
+#include "math.h"
+#include "R1370P.h"
+#include"hcsr04.h"
+#define RESOLUTION 500
+
+CAN can1(PB_5,PB_13);
+
+Ec2multi ec[]= {
+ Ec2multi(PC_5,PB_2,RESOLUTION),
+ Ec2multi(PA_11,PB_1,RESOLUTION),
+ Ec2multi(PB_12,PB_15,RESOLUTION),
+ Ec2multi(PC_4,PB_14,RESOLUTION)
+}; //2逓倍用class
+
+Ec2multi ecXY[]= {
+ Ec2multi(PC_6,PB_8,RESOLUTION),
+ Ec2multi(PC_8,PB_9,RESOLUTION)
+};
+
+SpeedControl motor[]= {
+ SpeedControl(PA_5,PC_7,50,ec[0]),
+ SpeedControl(PC_9,PA_1,50,ec[1]),
+ SpeedControl(PA_10,PB_4,50,ec[2]),
+ SpeedControl(PA_9,PA_7,50,ec[3])
+};
+
+DigitalIn button(USER_BUTTON);
+Serial pc(USBTX, USBRX); // tx, rx
+R1370P gyro(PC_10,PC_11); // tx, rx
+
+HCSR04 echo[]= {
+ HCSR04(PC_0,PC_12)//A
+ ,HCSR04(PA_15,PB_7)//A
+ ,HCSR04(PH_1,PB_0)// B
+ ,HCSR04(PC_3,PB_10)//B
+};
+
+Ticker ticker;
+
+//自己位置取得
+double theta=0;
+class Location
+{
+public:
+ Location():x_(0),y_(0)
+ {
+ for(int i =0; i<2; i++) {
+ old_count[i]=0;
+ }
+ }
+ void calXY()
+ {
+ double ec_count[2]= {};
+ double ax,ay,bx,by;
+ double atheta,btheta;
+ atheta = (45+theta)/180*3.14;
+ btheta = (135+theta)/180*3.14;
+
+ ec_count[0]=ecXY[0].getCount();
+ ec_count[1]=ecXY[1].getCount();
+ ax = (ec_count[0]-old_count[0])*cos(atheta);
+ ay = (ec_count[0]-old_count[0])*sin(atheta);
+ bx = (ec_count[1]-old_count[1])*cos(btheta);
+ by = (ec_count[1]-old_count[1])*sin(btheta);
+ x_=x_+ax + bx;
+ y_=y_+ay + by;
+ old_count[0]=ec_count[0];
+ old_count[1]=ec_count[1];
+ }
+ double getX()
+ {
+ return x_;
+ }
+ double getY()
+ {
+ return y_;
+ }
+
+private:
+ double x_;
+ double y_;
+ double old_count[2];
+};
+
+
+
+//目的地決定
+int plot[][2]= {
+ {0,0}
+ ,{800,13500}
+ ,{1000,15500}
+ ,{8654,16500}
+ ,{16000,16500}
+};
+
+double aimTheta[]= {//目標角度を指定
+ 0,0,0,0,0,0,0,0,0,0,0,0
+};
+
+double zMin[]= { //速度の最少を指定
+ 2,2,2, 2,2,2, 5,5,5, 5,5,5
+};
+
+//出力を計算
+int x,y;
+
+class WheelOmega
+{
+public:
+ WheelOmega(): max_(0),vx_(0),vy_(0),theta_(0)
+ {
+ for(int i=0; i<4; i++) {
+ omega[i]=0;
+ }
+ }
+ void setOmega(double max,double k)
+ {
+ max_=max;
+ k_=k;
+
+ }
+ void setVxy(double vx,double vy,double aimtheta_)
+ {
+ vx_=vx;
+ vy_=vy;
+ theta_=aimtheta_ - theta;
+ if(theta_>30) {//目標角度まで３０度以上空いていたら補正、係数調整のため３０は適当
+ theta_=30;
+ }
+ if(theta_<-30) {
+ theta_=-30;
+ }
+ }
+ void calOmega()
+ {
+ double theta_rad=45/180*3.14;
+ omega[0]=max_*vx_*cos(theta_rad)-max_*vy_*cos(theta_rad) + theta_*k_;
+ omega[1]=-max_*vx_*cos(theta_rad)-max_*vy_*cos(theta_rad)+theta_*k_;
+ omega[2]=-max_*vx_*cos(theta_rad)+max_*vy_*cos(theta_rad)+theta_*k_;
+ omega[3]=max_*vx_*cos(theta_rad)+max_*vy_*cos(theta_rad)+theta_*k_;
+ };
+ double getOmega(int i)
+ {
+ return omega[i];
+ }
+private:
+ double max_,vx_,vy_,theta_,k_;
+ double omega[4];
+};
+
+WheelOmega omega;
+//パラメタ処理
+double pControl(double distance_,double zMin,double newtime)
+{
+ double z,zMax,olddistance,oldtime;
+ double diftime_;
+ diftime_ = newtime - oldtime;
+ oldtime= newtime;
+ z=0.004*distance_ - 0.1*(olddistance-distance_)/diftime_;
+ zMax=2;
+ if(z>zMax) {
+ z=zMax;
+ }
+ if(z<zMin) {
+ z=zMin;
+ }
+ if(newtime<1) {
+ z=z*newtime;
+ }
+ olddistance = distance_;
+ return z;
+}
+
+//超音波
+class Sonic
+{
+private:
+ Sonic()
+ {
+ for(int i=0; i<4; i++) {
+ sonic_cm[i]=0;
+ }
+ }
+
+ void cal_sonic()
+ {
+ for(int i=0; i<4; i++) {
+ echo[i].start();
+ }
+ for(int i=0; i<4; i++) {
+ sonic_cm[i] =echo[i].get_dist_cm();
+ }
+ }
+ double get_sonic(int i)
+ {
+ return sonic_cm[i];
+ }
+
+private:
+ double sonic_cm[4];
+};
+
+
+//出力
+//int a=0;
+//int j=0;
+void motorOut()
+{
+ for(int i=0; i<4; i++) {
+ motor[i].Sc(omega.getOmega(i));
+ }
+}
+
+int main()
+{
+ can1.frequency(1000000);
+ gyro.initialize(); //main関数の最初に一度だけ実行
+ gyro.acc_offset();
+ double angle;
+ angle=gyro.getAngle();
+ double z;
+ printf("start\r\n");
+ motor[0].setEquation(0.008031,-0.022300,-0.008839,-0.016290);
+ motor[1].setEquation(0.008878,-0.016622,-0.009702,-0.015806);
+ motor[2].setEquation(0.008637,-0.016537,-0.009397,-0.012159);
+ motor[3].setEquation(0.008096,-0.014822,-0.008801,-0.016645);
+
+ motor[0].setDutyLimit(0.4);
+ motor[1].setDutyLimit(0.4);
+ motor[2].setDutyLimit(0.4);
+ motor[3].setDutyLimit(0.4);
+
+ motor[0].setPDparam( 0.1790, 0.00560);
+ motor[1].setPDparam( 0.1705, 0.00620);
+ motor[2].setPDparam( 0.1790, 0.00620);
+ motor[3].setPDparam( 0.1680, 0.00560);
+
+ while(1) {
+ printf("waiting\r\n");
+ if(button==0) {
+ wait(1);
+ ticker.attach(motorOut,0.05);
+ break;
+ }
+ }
+
+ int n=1,dx,dy,aimX,aimY;
+ double vx_,vy_,vx,vy,newtime,distance;
+ Location location;
+ Timer time;
+ time.start();
+ while(1) {
+ //自己位置取得
+ theta=gyro.getAngle()-angle; //角度の値を受け取る
+ location.calXY();
+
+ x=location.getX();
+ y=location.getY();
+ printf("X=%d,Y=%d,theta=%5.3f z=%5.3f %f\r\n",x,y,theta,z,time.read());
+
+ //目的地決定(syuusoku check)
+ aimX = plot[n][0];
+ aimY = plot[n][1];
+ //出力を計算(kitai xy);
+ dx=aimX-x;
+ dy=aimY-y;
+ vx_=dx/sqrt((double)dx*dx+dy*dy);
+ vy_=dy/sqrt((double)dx*dx+dy*dy);
+ vx=vx_*cos(theta/180*3.14)+vy_*sin(theta/180*3.14);
+ vy=-vx_*sin(theta/180*3.14)+vy_*cos(theta/180*3.14);
+ //四輪の出力計算
+ newtime=time.read();
+ distance = sqrt((float)dx*dx+dy*dy);
+ z=pControl(distance,zMin[n],newtime);
+ omega.setOmega(z,0.05);
+ omega.setVxy(vx,vy,aimTheta[n]);
+ omega.calOmega();
+ //ゴール判定
+ if(distance<800) {
+ n++;
+ printf("reach%d\r\n",n);
+ time.reset();
+ }
+
+ if(n>=5) {
+ for(int j=0; j<4; j++) {
+ motor[j].Sc(0);
+ }
+ printf("fin\r\n");
+ ticker.detach();
+ }
+ }
+}
\ No newline at end of file