春ロボ　ロケット団 / Mbed 2 deprecated HaruRobo

大会本番用

Dependencies:   mbed SpeedController hcsr04 Encoder CruizCore_R1370P

main.cpp@0:c0e9bbc27454, 2020-03-11 (annotated)

Committer:
maxnagazumi
Date:
Wed Mar 11 05:54:39 2020 +0000
Revision:
0:c0e9bbc27454
Child:
1:a692014d8e41
3/11

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maxnagazumi 0:c0e9bbc27454 1 #include "mbed.h"
maxnagazumi 0:c0e9bbc27454 2 #include "EC.h"
maxnagazumi 0:c0e9bbc27454 3 #include "SpeedController.h"
maxnagazumi 0:c0e9bbc27454 4 #include "math.h"
maxnagazumi 0:c0e9bbc27454 5 #include "R1370P.h"
maxnagazumi 0:c0e9bbc27454 6 #include"hcsr04.h"
maxnagazumi 0:c0e9bbc27454 7 #define RESOLUTION 500
maxnagazumi 0:c0e9bbc27454 8
maxnagazumi 0:c0e9bbc27454 9 CAN can1(PB_5,PB_13);
maxnagazumi 0:c0e9bbc27454 10
maxnagazumi 0:c0e9bbc27454 11 Ec2multi ec[]= {
maxnagazumi 0:c0e9bbc27454 12 Ec2multi(PC_5,PB_2,RESOLUTION),
maxnagazumi 0:c0e9bbc27454 13 Ec2multi(PA_11,PB_1,RESOLUTION),
maxnagazumi 0:c0e9bbc27454 14 Ec2multi(PB_12,PB_15,RESOLUTION),
maxnagazumi 0:c0e9bbc27454 15 Ec2multi(PC_4,PB_14,RESOLUTION)
maxnagazumi 0:c0e9bbc27454 16 }; //2逓倍用class
maxnagazumi 0:c0e9bbc27454 17
maxnagazumi 0:c0e9bbc27454 18 Ec2multi ecXY[]= {
maxnagazumi 0:c0e9bbc27454 19 Ec2multi(PC_6,PB_8,RESOLUTION),
maxnagazumi 0:c0e9bbc27454 20 Ec2multi(PC_8,PB_9,RESOLUTION)
maxnagazumi 0:c0e9bbc27454 21 };
maxnagazumi 0:c0e9bbc27454 22
maxnagazumi 0:c0e9bbc27454 23 SpeedControl motor[]= {
maxnagazumi 0:c0e9bbc27454 24 SpeedControl(PA_5,PC_7,50,ec[0]),
maxnagazumi 0:c0e9bbc27454 25 SpeedControl(PC_9,PA_1,50,ec[1]),
maxnagazumi 0:c0e9bbc27454 26 SpeedControl(PA_10,PB_4,50,ec[2]),
maxnagazumi 0:c0e9bbc27454 27 SpeedControl(PA_9,PA_7,50,ec[3])
maxnagazumi 0:c0e9bbc27454 28 };
maxnagazumi 0:c0e9bbc27454 29
maxnagazumi 0:c0e9bbc27454 30 DigitalIn button(USER_BUTTON);
maxnagazumi 0:c0e9bbc27454 31 Serial pc(USBTX, USBRX); // tx, rx
maxnagazumi 0:c0e9bbc27454 32 R1370P gyro(PC_10,PC_11); // tx, rx
maxnagazumi 0:c0e9bbc27454 33
maxnagazumi 0:c0e9bbc27454 34 HCSR04 echo[]= {
maxnagazumi 0:c0e9bbc27454 35 HCSR04(PC_0,PC_12)//A
maxnagazumi 0:c0e9bbc27454 36 ,HCSR04(PA_15,PB_7)//A
maxnagazumi 0:c0e9bbc27454 37 ,HCSR04(PH_1,PB_0)// B
maxnagazumi 0:c0e9bbc27454 38 ,HCSR04(PC_3,PB_10)//B
maxnagazumi 0:c0e9bbc27454 39 };
maxnagazumi 0:c0e9bbc27454 40
maxnagazumi 0:c0e9bbc27454 41 Ticker ticker;
maxnagazumi 0:c0e9bbc27454 42
maxnagazumi 0:c0e9bbc27454 43 //自己位置取得
maxnagazumi 0:c0e9bbc27454 44 double theta=0;
maxnagazumi 0:c0e9bbc27454 45 class Location
maxnagazumi 0:c0e9bbc27454 46 {
maxnagazumi 0:c0e9bbc27454 47 public:
maxnagazumi 0:c0e9bbc27454 48 Location():x_(0),y_(0)
maxnagazumi 0:c0e9bbc27454 49 {
maxnagazumi 0:c0e9bbc27454 50 for(int i =0; i<2; i++) {
maxnagazumi 0:c0e9bbc27454 51 old_count[i]=0;
maxnagazumi 0:c0e9bbc27454 52 }
maxnagazumi 0:c0e9bbc27454 53 }
maxnagazumi 0:c0e9bbc27454 54 void calXY()
maxnagazumi 0:c0e9bbc27454 55 {
maxnagazumi 0:c0e9bbc27454 56 double ec_count[2]= {};
maxnagazumi 0:c0e9bbc27454 57 double ax,ay,bx,by;
maxnagazumi 0:c0e9bbc27454 58 double atheta,btheta;
maxnagazumi 0:c0e9bbc27454 59 atheta = (45+theta)/180*3.14;
maxnagazumi 0:c0e9bbc27454 60 btheta = (135+theta)/180*3.14;
maxnagazumi 0:c0e9bbc27454 61
maxnagazumi 0:c0e9bbc27454 62 ec_count[0]=ecXY[0].getCount();
maxnagazumi 0:c0e9bbc27454 63 ec_count[1]=ecXY[1].getCount();
maxnagazumi 0:c0e9bbc27454 64 ax = (ec_count[0]-old_count[0])*cos(atheta);
maxnagazumi 0:c0e9bbc27454 65 ay = (ec_count[0]-old_count[0])*sin(atheta);
maxnagazumi 0:c0e9bbc27454 66 bx = (ec_count[1]-old_count[1])*cos(btheta);
maxnagazumi 0:c0e9bbc27454 67 by = (ec_count[1]-old_count[1])*sin(btheta);
maxnagazumi 0:c0e9bbc27454 68 x_=x_+ax + bx;
maxnagazumi 0:c0e9bbc27454 69 y_=y_+ay + by;
maxnagazumi 0:c0e9bbc27454 70 old_count[0]=ec_count[0];
maxnagazumi 0:c0e9bbc27454 71 old_count[1]=ec_count[1];
maxnagazumi 0:c0e9bbc27454 72 }
maxnagazumi 0:c0e9bbc27454 73 double getX()
maxnagazumi 0:c0e9bbc27454 74 {
maxnagazumi 0:c0e9bbc27454 75 return x_;
maxnagazumi 0:c0e9bbc27454 76 }
maxnagazumi 0:c0e9bbc27454 77 double getY()
maxnagazumi 0:c0e9bbc27454 78 {
maxnagazumi 0:c0e9bbc27454 79 return y_;
maxnagazumi 0:c0e9bbc27454 80 }
maxnagazumi 0:c0e9bbc27454 81
maxnagazumi 0:c0e9bbc27454 82 private:
maxnagazumi 0:c0e9bbc27454 83 double x_;
maxnagazumi 0:c0e9bbc27454 84 double y_;
maxnagazumi 0:c0e9bbc27454 85 double old_count[2];
maxnagazumi 0:c0e9bbc27454 86 };
maxnagazumi 0:c0e9bbc27454 87
maxnagazumi 0:c0e9bbc27454 88
maxnagazumi 0:c0e9bbc27454 89
maxnagazumi 0:c0e9bbc27454 90 //目的地決定
maxnagazumi 0:c0e9bbc27454 91 int plot[][2]= {
maxnagazumi 0:c0e9bbc27454 92 {0,0}
maxnagazumi 0:c0e9bbc27454 93 ,{800,13500}
maxnagazumi 0:c0e9bbc27454 94 ,{1000,15500}
maxnagazumi 0:c0e9bbc27454 95 ,{8654,16500}
maxnagazumi 0:c0e9bbc27454 96 ,{16000,16500}
maxnagazumi 0:c0e9bbc27454 97 };
maxnagazumi 0:c0e9bbc27454 98
maxnagazumi 0:c0e9bbc27454 99 double aimTheta[]= {//目標角度を指定
maxnagazumi 0:c0e9bbc27454 100 0,0,0,0,0,0,0,0,0,0,0,0
maxnagazumi 0:c0e9bbc27454 101 };
maxnagazumi 0:c0e9bbc27454 102
maxnagazumi 0:c0e9bbc27454 103 double zMin[]= { //速度の最少を指定
maxnagazumi 0:c0e9bbc27454 104 2,2,2, 2,2,2, 5,5,5, 5,5,5
maxnagazumi 0:c0e9bbc27454 105 };
maxnagazumi 0:c0e9bbc27454 106
maxnagazumi 0:c0e9bbc27454 107 //出力を計算
maxnagazumi 0:c0e9bbc27454 108 int x,y;
maxnagazumi 0:c0e9bbc27454 109
maxnagazumi 0:c0e9bbc27454 110 class WheelOmega
maxnagazumi 0:c0e9bbc27454 111 {
maxnagazumi 0:c0e9bbc27454 112 public:
maxnagazumi 0:c0e9bbc27454 113 WheelOmega(): max_(0),vx_(0),vy_(0),theta_(0)
maxnagazumi 0:c0e9bbc27454 114 {
maxnagazumi 0:c0e9bbc27454 115 for(int i=0; i<4; i++) {
maxnagazumi 0:c0e9bbc27454 116 omega[i]=0;
maxnagazumi 0:c0e9bbc27454 117 }
maxnagazumi 0:c0e9bbc27454 118 }
maxnagazumi 0:c0e9bbc27454 119 void setOmega(double max,double k)
maxnagazumi 0:c0e9bbc27454 120 {
maxnagazumi 0:c0e9bbc27454 121 max_=max;
maxnagazumi 0:c0e9bbc27454 122 k_=k;
maxnagazumi 0:c0e9bbc27454 123
maxnagazumi 0:c0e9bbc27454 124 }
maxnagazumi 0:c0e9bbc27454 125 void setVxy(double vx,double vy,double aimtheta_)
maxnagazumi 0:c0e9bbc27454 126 {
maxnagazumi 0:c0e9bbc27454 127 vx_=vx;
maxnagazumi 0:c0e9bbc27454 128 vy_=vy;
maxnagazumi 0:c0e9bbc27454 129 theta_=aimtheta_ - theta;
maxnagazumi 0:c0e9bbc27454 130 if(theta_>30) {//目標角度まで３０度以上空いていたら補正、係数調整のため３０は適当
maxnagazumi 0:c0e9bbc27454 131 theta_=30;
maxnagazumi 0:c0e9bbc27454 132 }
maxnagazumi 0:c0e9bbc27454 133 if(theta_<-30) {
maxnagazumi 0:c0e9bbc27454 134 theta_=-30;
maxnagazumi 0:c0e9bbc27454 135 }
maxnagazumi 0:c0e9bbc27454 136 }
maxnagazumi 0:c0e9bbc27454 137 void calOmega()
maxnagazumi 0:c0e9bbc27454 138 {
maxnagazumi 0:c0e9bbc27454 139 double theta_rad=45/180*3.14;
maxnagazumi 0:c0e9bbc27454 140 omega[0]=max_*vx_*cos(theta_rad)-max_*vy_*cos(theta_rad) + theta_*k_;
maxnagazumi 0:c0e9bbc27454 141 omega[1]=-max_*vx_*cos(theta_rad)-max_*vy_*cos(theta_rad)+theta_*k_;
maxnagazumi 0:c0e9bbc27454 142 omega[2]=-max_*vx_*cos(theta_rad)+max_*vy_*cos(theta_rad)+theta_*k_;
maxnagazumi 0:c0e9bbc27454 143 omega[3]=max_*vx_*cos(theta_rad)+max_*vy_*cos(theta_rad)+theta_*k_;
maxnagazumi 0:c0e9bbc27454 144 };
maxnagazumi 0:c0e9bbc27454 145 double getOmega(int i)
maxnagazumi 0:c0e9bbc27454 146 {
maxnagazumi 0:c0e9bbc27454 147 return omega[i];
maxnagazumi 0:c0e9bbc27454 148 }
maxnagazumi 0:c0e9bbc27454 149 private:
maxnagazumi 0:c0e9bbc27454 150 double max_,vx_,vy_,theta_,k_;
maxnagazumi 0:c0e9bbc27454 151 double omega[4];
maxnagazumi 0:c0e9bbc27454 152 };
maxnagazumi 0:c0e9bbc27454 153
maxnagazumi 0:c0e9bbc27454 154 WheelOmega omega;
maxnagazumi 0:c0e9bbc27454 155 //パラメタ処理
maxnagazumi 0:c0e9bbc27454 156 double pControl(double distance_,double zMin,double newtime)
maxnagazumi 0:c0e9bbc27454 157 {
maxnagazumi 0:c0e9bbc27454 158 double z,zMax,olddistance,oldtime;
maxnagazumi 0:c0e9bbc27454 159 double diftime_;
maxnagazumi 0:c0e9bbc27454 160 diftime_ = newtime - oldtime;
maxnagazumi 0:c0e9bbc27454 161 oldtime= newtime;
maxnagazumi 0:c0e9bbc27454 162 z=0.004*distance_ - 0.1*(olddistance-distance_)/diftime_;
maxnagazumi 0:c0e9bbc27454 163 zMax=2;
maxnagazumi 0:c0e9bbc27454 164 if(z>zMax) {
maxnagazumi 0:c0e9bbc27454 165 z=zMax;
maxnagazumi 0:c0e9bbc27454 166 }
maxnagazumi 0:c0e9bbc27454 167 if(z<zMin) {
maxnagazumi 0:c0e9bbc27454 168 z=zMin;
maxnagazumi 0:c0e9bbc27454 169 }
maxnagazumi 0:c0e9bbc27454 170 if(newtime<1) {
maxnagazumi 0:c0e9bbc27454 171 z=z*newtime;
maxnagazumi 0:c0e9bbc27454 172 }
maxnagazumi 0:c0e9bbc27454 173 olddistance = distance_;
maxnagazumi 0:c0e9bbc27454 174 return z;
maxnagazumi 0:c0e9bbc27454 175 }
maxnagazumi 0:c0e9bbc27454 176
maxnagazumi 0:c0e9bbc27454 177 //超音波
maxnagazumi 0:c0e9bbc27454 178 class Sonic
maxnagazumi 0:c0e9bbc27454 179 {
maxnagazumi 0:c0e9bbc27454 180 private:
maxnagazumi 0:c0e9bbc27454 181 Sonic()
maxnagazumi 0:c0e9bbc27454 182 {
maxnagazumi 0:c0e9bbc27454 183 for(int i=0; i<4; i++) {
maxnagazumi 0:c0e9bbc27454 184 sonic_cm[i]=0;
maxnagazumi 0:c0e9bbc27454 185 }
maxnagazumi 0:c0e9bbc27454 186 }
maxnagazumi 0:c0e9bbc27454 187
maxnagazumi 0:c0e9bbc27454 188 void cal_sonic()
maxnagazumi 0:c0e9bbc27454 189 {
maxnagazumi 0:c0e9bbc27454 190 for(int i=0; i<4; i++) {
maxnagazumi 0:c0e9bbc27454 191 echo[i].start();
maxnagazumi 0:c0e9bbc27454 192 }
maxnagazumi 0:c0e9bbc27454 193 for(int i=0; i<4; i++) {
maxnagazumi 0:c0e9bbc27454 194 sonic_cm[i] =echo[i].get_dist_cm();
maxnagazumi 0:c0e9bbc27454 195 }
maxnagazumi 0:c0e9bbc27454 196 }
maxnagazumi 0:c0e9bbc27454 197 double get_sonic(int i)
maxnagazumi 0:c0e9bbc27454 198 {
maxnagazumi 0:c0e9bbc27454 199 return sonic_cm[i];
maxnagazumi 0:c0e9bbc27454 200 }
maxnagazumi 0:c0e9bbc27454 201
maxnagazumi 0:c0e9bbc27454 202 private:
maxnagazumi 0:c0e9bbc27454 203 double sonic_cm[4];
maxnagazumi 0:c0e9bbc27454 204 };
maxnagazumi 0:c0e9bbc27454 205
maxnagazumi 0:c0e9bbc27454 206
maxnagazumi 0:c0e9bbc27454 207 //出力
maxnagazumi 0:c0e9bbc27454 208 //int a=0;
maxnagazumi 0:c0e9bbc27454 209 //int j=0;
maxnagazumi 0:c0e9bbc27454 210 void motorOut()
maxnagazumi 0:c0e9bbc27454 211 {
maxnagazumi 0:c0e9bbc27454 212 for(int i=0; i<4; i++) {
maxnagazumi 0:c0e9bbc27454 213 motor[i].Sc(omega.getOmega(i));
maxnagazumi 0:c0e9bbc27454 214 }
maxnagazumi 0:c0e9bbc27454 215 }
maxnagazumi 0:c0e9bbc27454 216
maxnagazumi 0:c0e9bbc27454 217 int main()
maxnagazumi 0:c0e9bbc27454 218 {
maxnagazumi 0:c0e9bbc27454 219 can1.frequency(1000000);
maxnagazumi 0:c0e9bbc27454 220 gyro.initialize(); //main関数の最初に一度だけ実行
maxnagazumi 0:c0e9bbc27454 221 gyro.acc_offset();
maxnagazumi 0:c0e9bbc27454 222 double angle;
maxnagazumi 0:c0e9bbc27454 223 angle=gyro.getAngle();
maxnagazumi 0:c0e9bbc27454 224 double z;
maxnagazumi 0:c0e9bbc27454 225 printf("start\r\n");
maxnagazumi 0:c0e9bbc27454 226 motor[0].setEquation(0.008031,-0.022300,-0.008839,-0.016290);
maxnagazumi 0:c0e9bbc27454 227 motor[1].setEquation(0.008878,-0.016622,-0.009702,-0.015806);
maxnagazumi 0:c0e9bbc27454 228 motor[2].setEquation(0.008637,-0.016537,-0.009397,-0.012159);
maxnagazumi 0:c0e9bbc27454 229 motor[3].setEquation(0.008096,-0.014822,-0.008801,-0.016645);
maxnagazumi 0:c0e9bbc27454 230
maxnagazumi 0:c0e9bbc27454 231 motor[0].setDutyLimit(0.4);
maxnagazumi 0:c0e9bbc27454 232 motor[1].setDutyLimit(0.4);
maxnagazumi 0:c0e9bbc27454 233 motor[2].setDutyLimit(0.4);
maxnagazumi 0:c0e9bbc27454 234 motor[3].setDutyLimit(0.4);
maxnagazumi 0:c0e9bbc27454 235
maxnagazumi 0:c0e9bbc27454 236 motor[0].setPDparam( 0.1790, 0.00560);
maxnagazumi 0:c0e9bbc27454 237 motor[1].setPDparam( 0.1705, 0.00620);
maxnagazumi 0:c0e9bbc27454 238 motor[2].setPDparam( 0.1790, 0.00620);
maxnagazumi 0:c0e9bbc27454 239 motor[3].setPDparam( 0.1680, 0.00560);
maxnagazumi 0:c0e9bbc27454 240
maxnagazumi 0:c0e9bbc27454 241 while(1) {
maxnagazumi 0:c0e9bbc27454 242 printf("waiting\r\n");
maxnagazumi 0:c0e9bbc27454 243 if(button==0) {
maxnagazumi 0:c0e9bbc27454 244 wait(1);
maxnagazumi 0:c0e9bbc27454 245 ticker.attach(motorOut,0.05);
maxnagazumi 0:c0e9bbc27454 246 break;
maxnagazumi 0:c0e9bbc27454 247 }
maxnagazumi 0:c0e9bbc27454 248 }
maxnagazumi 0:c0e9bbc27454 249
maxnagazumi 0:c0e9bbc27454 250 int n=1,dx,dy,aimX,aimY;
maxnagazumi 0:c0e9bbc27454 251 double vx_,vy_,vx,vy,newtime,distance;
maxnagazumi 0:c0e9bbc27454 252 Location location;
maxnagazumi 0:c0e9bbc27454 253 Timer time;
maxnagazumi 0:c0e9bbc27454 254 time.start();
maxnagazumi 0:c0e9bbc27454 255 while(1) {
maxnagazumi 0:c0e9bbc27454 256 //自己位置取得
maxnagazumi 0:c0e9bbc27454 257 theta=gyro.getAngle()-angle; //角度の値を受け取る
maxnagazumi 0:c0e9bbc27454 258 location.calXY();
maxnagazumi 0:c0e9bbc27454 259
maxnagazumi 0:c0e9bbc27454 260 x=location.getX();
maxnagazumi 0:c0e9bbc27454 261 y=location.getY();
maxnagazumi 0:c0e9bbc27454 262 printf("X=%d,Y=%d,theta=%5.3f z=%5.3f %f\r\n",x,y,theta,z,time.read());
maxnagazumi 0:c0e9bbc27454 263
maxnagazumi 0:c0e9bbc27454 264 //目的地決定(syuusoku check)
maxnagazumi 0:c0e9bbc27454 265 aimX = plot[n][0];
maxnagazumi 0:c0e9bbc27454 266 aimY = plot[n][1];
maxnagazumi 0:c0e9bbc27454 267 //出力を計算(kitai xy);
maxnagazumi 0:c0e9bbc27454 268 dx=aimX-x;
maxnagazumi 0:c0e9bbc27454 269 dy=aimY-y;
maxnagazumi 0:c0e9bbc27454 270 vx_=dx/sqrt((double)dx*dx+dy*dy);
maxnagazumi 0:c0e9bbc27454 271 vy_=dy/sqrt((double)dx*dx+dy*dy);
maxnagazumi 0:c0e9bbc27454 272 vx=vx_*cos(theta/180*3.14)+vy_*sin(theta/180*3.14);
maxnagazumi 0:c0e9bbc27454 273 vy=-vx_*sin(theta/180*3.14)+vy_*cos(theta/180*3.14);
maxnagazumi 0:c0e9bbc27454 274 //四輪の出力計算
maxnagazumi 0:c0e9bbc27454 275 newtime=time.read();
maxnagazumi 0:c0e9bbc27454 276 distance = sqrt((float)dx*dx+dy*dy);
maxnagazumi 0:c0e9bbc27454 277 z=pControl(distance,zMin[n],newtime);
maxnagazumi 0:c0e9bbc27454 278 omega.setOmega(z,0.05);
maxnagazumi 0:c0e9bbc27454 279 omega.setVxy(vx,vy,aimTheta[n]);
maxnagazumi 0:c0e9bbc27454 280 omega.calOmega();
maxnagazumi 0:c0e9bbc27454 281 //ゴール判定
maxnagazumi 0:c0e9bbc27454 282 if(distance<800) {
maxnagazumi 0:c0e9bbc27454 283 n++;
maxnagazumi 0:c0e9bbc27454 284 printf("reach%d\r\n",n);
maxnagazumi 0:c0e9bbc27454 285 time.reset();
maxnagazumi 0:c0e9bbc27454 286 }
maxnagazumi 0:c0e9bbc27454 287
maxnagazumi 0:c0e9bbc27454 288 if(n>=5) {
maxnagazumi 0:c0e9bbc27454 289 for(int j=0; j<4; j++) {
maxnagazumi 0:c0e9bbc27454 290 motor[j].Sc(0);
maxnagazumi 0:c0e9bbc27454 291 }
maxnagazumi 0:c0e9bbc27454 292 printf("fin\r\n");
maxnagazumi 0:c0e9bbc27454 293 ticker.detach();
maxnagazumi 0:c0e9bbc27454 294 }
maxnagazumi 0:c0e9bbc27454 295 }
maxnagazumi 0:c0e9bbc27454 296 }