![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
大会本番用
Dependencies: mbed SpeedController hcsr04 Encoder CruizCore_R1370P
Diff: main.cpp
- Revision:
- 0:c0e9bbc27454
- Child:
- 1:a692014d8e41
diff -r 000000000000 -r c0e9bbc27454 main.cpp --- /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) {//目標角度まで30度以上空いていたら補正、係数調整のため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