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

大会本番用

Dependencies:   mbed SpeedController hcsr04 Encoder CruizCore_R1370P

main.cpp@2:0be4cfbdf408, 2020-03-27 (annotated)

Committer:
maxnagazumi
Date:
Fri Mar 27 06:31:20 2020 +0000
Revision:
2:0be4cfbdf408
Parent:
1:a692014d8e41 
soho;

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 1:a692014d8e41 10 /*
maxnagazumi 2:0be4cfbdf408 11 0 start/stop
maxnagazumi 1:a692014d8e41 12 1 to 4 , 6 to 9
maxnagazumi 1:a692014d8e41 13 5 stop
maxnagazumi 1:a692014d8e41 14 10 clock move
maxnagazumi 1:a692014d8e41 15 11 not clock move
maxnagazumi 1:a692014d8e41 16 12 speed change
maxnagazumi 1:a692014d8e41 17 */
maxnagazumi 0:c0e9bbc27454 18 Ec2multi ec[]= {
maxnagazumi 0:c0e9bbc27454 19 Ec2multi(PC_5,PB_2,RESOLUTION),
maxnagazumi 0:c0e9bbc27454 20 Ec2multi(PA_11,PB_1,RESOLUTION),
maxnagazumi 0:c0e9bbc27454 21 Ec2multi(PB_12,PB_15,RESOLUTION),
maxnagazumi 0:c0e9bbc27454 22 Ec2multi(PC_4,PB_14,RESOLUTION)
maxnagazumi 0:c0e9bbc27454 23 }; //2逓倍用class
maxnagazumi 0:c0e9bbc27454 24
maxnagazumi 0:c0e9bbc27454 25 Ec2multi ecXY[]= {
maxnagazumi 0:c0e9bbc27454 26 Ec2multi(PC_6,PB_8,RESOLUTION),
maxnagazumi 0:c0e9bbc27454 27 Ec2multi(PC_8,PB_9,RESOLUTION)
maxnagazumi 0:c0e9bbc27454 28 };
maxnagazumi 0:c0e9bbc27454 29
maxnagazumi 0:c0e9bbc27454 30 SpeedControl motor[]= {
maxnagazumi 0:c0e9bbc27454 31 SpeedControl(PA_5,PC_7,50,ec[0]),
maxnagazumi 0:c0e9bbc27454 32 SpeedControl(PC_9,PA_1,50,ec[1]),
maxnagazumi 0:c0e9bbc27454 33 SpeedControl(PA_10,PB_4,50,ec[2]),
maxnagazumi 0:c0e9bbc27454 34 SpeedControl(PA_9,PA_7,50,ec[3])
maxnagazumi 0:c0e9bbc27454 35 };
maxnagazumi 0:c0e9bbc27454 36
maxnagazumi 0:c0e9bbc27454 37 DigitalIn button(USER_BUTTON);
maxnagazumi 0:c0e9bbc27454 38 Serial pc(USBTX, USBRX); // tx, rx
maxnagazumi 0:c0e9bbc27454 39 R1370P gyro(PC_10,PC_11); // tx, rx
maxnagazumi 0:c0e9bbc27454 40
maxnagazumi 2:0be4cfbdf408 41 /*HCSR04 echo[]= {
maxnagazumi 0:c0e9bbc27454 42 HCSR04(PC_0,PC_12)//A
maxnagazumi 0:c0e9bbc27454 43 ,HCSR04(PA_15,PB_7)//A
maxnagazumi 0:c0e9bbc27454 44 ,HCSR04(PH_1,PB_0)// B
maxnagazumi 0:c0e9bbc27454 45 ,HCSR04(PC_3,PB_10)//B
maxnagazumi 2:0be4cfbdf408 46 };*/
maxnagazumi 0:c0e9bbc27454 47
maxnagazumi 1:a692014d8e41 48 //自己位置取得
maxnagazumi 1:a692014d8e41 49 double theta;
maxnagazumi 1:a692014d8e41 50 double angle;
maxnagazumi 0:c0e9bbc27454 51
maxnagazumi 0:c0e9bbc27454 52 class Location
maxnagazumi 0:c0e9bbc27454 53 {
maxnagazumi 0:c0e9bbc27454 54 public:
maxnagazumi 0:c0e9bbc27454 55 Location():x_(0),y_(0)
maxnagazumi 0:c0e9bbc27454 56 {
maxnagazumi 0:c0e9bbc27454 57 for(int i =0; i<2; i++) {
maxnagazumi 0:c0e9bbc27454 58 old_count[i]=0;
maxnagazumi 0:c0e9bbc27454 59 }
maxnagazumi 0:c0e9bbc27454 60 }
maxnagazumi 0:c0e9bbc27454 61 void calXY()
maxnagazumi 0:c0e9bbc27454 62 {
maxnagazumi 0:c0e9bbc27454 63 double ec_count[2]= {};
maxnagazumi 0:c0e9bbc27454 64 double ax,ay,bx,by;
maxnagazumi 0:c0e9bbc27454 65 double atheta,btheta;
maxnagazumi 0:c0e9bbc27454 66 atheta = (45+theta)/180*3.14;
maxnagazumi 0:c0e9bbc27454 67 btheta = (135+theta)/180*3.14;
maxnagazumi 0:c0e9bbc27454 68
maxnagazumi 0:c0e9bbc27454 69 ec_count[0]=ecXY[0].getCount();
maxnagazumi 0:c0e9bbc27454 70 ec_count[1]=ecXY[1].getCount();
maxnagazumi 0:c0e9bbc27454 71 ax = (ec_count[0]-old_count[0])*cos(atheta);
maxnagazumi 0:c0e9bbc27454 72 ay = (ec_count[0]-old_count[0])*sin(atheta);
maxnagazumi 0:c0e9bbc27454 73 bx = (ec_count[1]-old_count[1])*cos(btheta);
maxnagazumi 0:c0e9bbc27454 74 by = (ec_count[1]-old_count[1])*sin(btheta);
maxnagazumi 0:c0e9bbc27454 75 x_=x_+ax + bx;
maxnagazumi 0:c0e9bbc27454 76 y_=y_+ay + by;
maxnagazumi 0:c0e9bbc27454 77 old_count[0]=ec_count[0];
maxnagazumi 0:c0e9bbc27454 78 old_count[1]=ec_count[1];
maxnagazumi 0:c0e9bbc27454 79 }
maxnagazumi 0:c0e9bbc27454 80 double getX()
maxnagazumi 0:c0e9bbc27454 81 {
maxnagazumi 0:c0e9bbc27454 82 return x_;
maxnagazumi 0:c0e9bbc27454 83 }
maxnagazumi 0:c0e9bbc27454 84 double getY()
maxnagazumi 0:c0e9bbc27454 85 {
maxnagazumi 0:c0e9bbc27454 86 return y_;
maxnagazumi 0:c0e9bbc27454 87 }
maxnagazumi 0:c0e9bbc27454 88
maxnagazumi 0:c0e9bbc27454 89 private:
maxnagazumi 0:c0e9bbc27454 90 double x_;
maxnagazumi 0:c0e9bbc27454 91 double y_;
maxnagazumi 0:c0e9bbc27454 92 double old_count[2];
maxnagazumi 0:c0e9bbc27454 93 };
maxnagazumi 0:c0e9bbc27454 94
maxnagazumi 0:c0e9bbc27454 95
maxnagazumi 0:c0e9bbc27454 96
maxnagazumi 0:c0e9bbc27454 97 //目的地決定
maxnagazumi 0:c0e9bbc27454 98 int plot[][2]= {
maxnagazumi 0:c0e9bbc27454 99 {0,0}
maxnagazumi 2:0be4cfbdf408 100 ,{2000,16000}
maxnagazumi 2:0be4cfbdf408 101 ,{8000,16000}
maxnagazumi 2:0be4cfbdf408 102 ,{4000,0}
maxnagazumi 2:0be4cfbdf408 103 ,{0,0}
maxnagazumi 2:0be4cfbdf408 104 ,{0,4000}
maxnagazumi 2:0be4cfbdf408 105 ,{4000,4000}
maxnagazumi 2:0be4cfbdf408 106 ,{4000,0}
maxnagazumi 2:0be4cfbdf408 107 ,{0,0}
maxnagazumi 0:c0e9bbc27454 108 };
maxnagazumi 0:c0e9bbc27454 109
maxnagazumi 0:c0e9bbc27454 110 double aimTheta[]= {//目標角度を指定
maxnagazumi 0:c0e9bbc27454 111 0,0,0,0,0,0,0,0,0,0,0,0
maxnagazumi 0:c0e9bbc27454 112 };
maxnagazumi 0:c0e9bbc27454 113
maxnagazumi 0:c0e9bbc27454 114 double zMin[]= { //速度の最少を指定
maxnagazumi 2:0be4cfbdf408 115 0,0,0,0,0,0,0,0,0,0,0,0
maxnagazumi 0:c0e9bbc27454 116 };
maxnagazumi 0:c0e9bbc27454 117
maxnagazumi 0:c0e9bbc27454 118 //出力を計算
maxnagazumi 0:c0e9bbc27454 119 int x,y;
maxnagazumi 0:c0e9bbc27454 120
maxnagazumi 0:c0e9bbc27454 121 class WheelOmega
maxnagazumi 0:c0e9bbc27454 122 {
maxnagazumi 0:c0e9bbc27454 123 public:
maxnagazumi 0:c0e9bbc27454 124 WheelOmega(): max_(0),vx_(0),vy_(0),theta_(0)
maxnagazumi 0:c0e9bbc27454 125 {
maxnagazumi 0:c0e9bbc27454 126 for(int i=0; i<4; i++) {
maxnagazumi 0:c0e9bbc27454 127 omega[i]=0;
maxnagazumi 0:c0e9bbc27454 128 }
maxnagazumi 0:c0e9bbc27454 129 }
maxnagazumi 0:c0e9bbc27454 130 void setOmega(double max,double k)
maxnagazumi 0:c0e9bbc27454 131 {
maxnagazumi 0:c0e9bbc27454 132 max_=max;
maxnagazumi 0:c0e9bbc27454 133 k_=k;
maxnagazumi 0:c0e9bbc27454 134
maxnagazumi 0:c0e9bbc27454 135 }
maxnagazumi 0:c0e9bbc27454 136 void setVxy(double vx,double vy,double aimtheta_)
maxnagazumi 0:c0e9bbc27454 137 {
maxnagazumi 0:c0e9bbc27454 138 vx_=vx;
maxnagazumi 0:c0e9bbc27454 139 vy_=vy;
maxnagazumi 0:c0e9bbc27454 140 theta_=aimtheta_ - theta;
maxnagazumi 0:c0e9bbc27454 141 if(theta_>30) {//目標角度まで３０度以上空いていたら補正、係数調整のため３０は適当
maxnagazumi 0:c0e9bbc27454 142 theta_=30;
maxnagazumi 0:c0e9bbc27454 143 }
maxnagazumi 0:c0e9bbc27454 144 if(theta_<-30) {
maxnagazumi 0:c0e9bbc27454 145 theta_=-30;
maxnagazumi 0:c0e9bbc27454 146 }
maxnagazumi 0:c0e9bbc27454 147 }
maxnagazumi 0:c0e9bbc27454 148 void calOmega()
maxnagazumi 0:c0e9bbc27454 149 {
maxnagazumi 0:c0e9bbc27454 150 double theta_rad=45/180*3.14;
maxnagazumi 0:c0e9bbc27454 151 omega[0]=max_*vx_*cos(theta_rad)-max_*vy_*cos(theta_rad) + theta_*k_;
maxnagazumi 0:c0e9bbc27454 152 omega[1]=-max_*vx_*cos(theta_rad)-max_*vy_*cos(theta_rad)+theta_*k_;
maxnagazumi 0:c0e9bbc27454 153 omega[2]=-max_*vx_*cos(theta_rad)+max_*vy_*cos(theta_rad)+theta_*k_;
maxnagazumi 0:c0e9bbc27454 154 omega[3]=max_*vx_*cos(theta_rad)+max_*vy_*cos(theta_rad)+theta_*k_;
maxnagazumi 0:c0e9bbc27454 155 };
maxnagazumi 0:c0e9bbc27454 156 double getOmega(int i)
maxnagazumi 0:c0e9bbc27454 157 {
maxnagazumi 0:c0e9bbc27454 158 return omega[i];
maxnagazumi 0:c0e9bbc27454 159 }
maxnagazumi 0:c0e9bbc27454 160 private:
maxnagazumi 0:c0e9bbc27454 161 double max_,vx_,vy_,theta_,k_;
maxnagazumi 0:c0e9bbc27454 162 double omega[4];
maxnagazumi 0:c0e9bbc27454 163 };
maxnagazumi 0:c0e9bbc27454 164
maxnagazumi 0:c0e9bbc27454 165 WheelOmega omega;
maxnagazumi 0:c0e9bbc27454 166 //パラメタ処理
maxnagazumi 0:c0e9bbc27454 167 double pControl(double distance_,double zMin,double newtime)
maxnagazumi 0:c0e9bbc27454 168 {
maxnagazumi 0:c0e9bbc27454 169 double z,zMax,olddistance,oldtime;
maxnagazumi 0:c0e9bbc27454 170 double diftime_;
maxnagazumi 0:c0e9bbc27454 171 diftime_ = newtime - oldtime;
maxnagazumi 0:c0e9bbc27454 172 oldtime= newtime;
maxnagazumi 0:c0e9bbc27454 173 z=0.004*distance_ - 0.1*(olddistance-distance_)/diftime_;
maxnagazumi 0:c0e9bbc27454 174 zMax=2;
maxnagazumi 0:c0e9bbc27454 175 if(z>zMax) {
maxnagazumi 0:c0e9bbc27454 176 z=zMax;
maxnagazumi 0:c0e9bbc27454 177 }
maxnagazumi 0:c0e9bbc27454 178 if(z<zMin) {
maxnagazumi 0:c0e9bbc27454 179 z=zMin;
maxnagazumi 0:c0e9bbc27454 180 }
maxnagazumi 0:c0e9bbc27454 181 if(newtime<1) {
maxnagazumi 0:c0e9bbc27454 182 z=z*newtime;
maxnagazumi 0:c0e9bbc27454 183 }
maxnagazumi 0:c0e9bbc27454 184 olddistance = distance_;
maxnagazumi 0:c0e9bbc27454 185 return z;
maxnagazumi 0:c0e9bbc27454 186 }
maxnagazumi 0:c0e9bbc27454 187
maxnagazumi 0:c0e9bbc27454 188 //超音波
maxnagazumi 2:0be4cfbdf408 189 /*class Sonic
maxnagazumi 0:c0e9bbc27454 190 {
maxnagazumi 1:a692014d8e41 191 public:
maxnagazumi 0:c0e9bbc27454 192 Sonic()
maxnagazumi 0:c0e9bbc27454 193 {
maxnagazumi 0:c0e9bbc27454 194 for(int i=0; i<4; i++) {
maxnagazumi 0:c0e9bbc27454 195 sonic_cm[i]=0;
maxnagazumi 0:c0e9bbc27454 196 }
maxnagazumi 0:c0e9bbc27454 197 }
maxnagazumi 0:c0e9bbc27454 198
maxnagazumi 0:c0e9bbc27454 199 void cal_sonic()
maxnagazumi 0:c0e9bbc27454 200 {
maxnagazumi 0:c0e9bbc27454 201 for(int i=0; i<4; i++) {
maxnagazumi 0:c0e9bbc27454 202 echo[i].start();
maxnagazumi 0:c0e9bbc27454 203 }
maxnagazumi 0:c0e9bbc27454 204 for(int i=0; i<4; i++) {
maxnagazumi 0:c0e9bbc27454 205 sonic_cm[i] =echo[i].get_dist_cm();
maxnagazumi 0:c0e9bbc27454 206 }
maxnagazumi 0:c0e9bbc27454 207 }
maxnagazumi 0:c0e9bbc27454 208 double get_sonic(int i)
maxnagazumi 0:c0e9bbc27454 209 {
maxnagazumi 0:c0e9bbc27454 210 return sonic_cm[i];
maxnagazumi 0:c0e9bbc27454 211 }
maxnagazumi 0:c0e9bbc27454 212
maxnagazumi 0:c0e9bbc27454 213 private:
maxnagazumi 0:c0e9bbc27454 214 double sonic_cm[4];
maxnagazumi 0:c0e9bbc27454 215 };
maxnagazumi 0:c0e9bbc27454 216
maxnagazumi 2:0be4cfbdf408 217 */
maxnagazumi 1:a692014d8e41 218 class CAN_ticker
maxnagazumi 1:a692014d8e41 219 {
maxnagazumi 1:a692014d8e41 220 public:
maxnagazumi 2:0be4cfbdf408 221 CAN_ticker():x(0)
maxnagazumi 2:0be4cfbdf408 222 {
maxnagazumi 2:0be4cfbdf408 223 data[0]=0;
maxnagazumi 2:0be4cfbdf408 224 }
maxnagazumi 2:0be4cfbdf408 225 void canmsg_read()
maxnagazumi 1:a692014d8e41 226 {
maxnagazumi 1:a692014d8e41 227 CANMessage msg;
maxnagazumi 1:a692014d8e41 228 if(can1.read(msg)) {
maxnagazumi 1:a692014d8e41 229 if(msg.id == 1) {
maxnagazumi 2:0be4cfbdf408 230 x=(short)(msg.data[0]);
maxnagazumi 1:a692014d8e41 231 }
maxnagazumi 1:a692014d8e41 232 }
maxnagazumi 1:a692014d8e41 233 }
maxnagazumi 1:a692014d8e41 234 int get_xCAN()
maxnagazumi 1:a692014d8e41 235 {
maxnagazumi 1:a692014d8e41 236 return x;
maxnagazumi 1:a692014d8e41 237 }
maxnagazumi 1:a692014d8e41 238 private:
maxnagazumi 2:0be4cfbdf408 239 char data[0];
maxnagazumi 1:a692014d8e41 240 int x;
maxnagazumi 1:a692014d8e41 241 };
maxnagazumi 1:a692014d8e41 242 //手動出力
maxnagazumi 2:0be4cfbdf408 243 double canOmega[4]= {
maxnagazumi 2:0be4cfbdf408 244 0,0,0,0
maxnagazumi 2:0be4cfbdf408 245 };
maxnagazumi 1:a692014d8e41 246 void calOmega_CAN(int canx)
maxnagazumi 1:a692014d8e41 247 {
maxnagazumi 2:0be4cfbdf408 248 static double a=0.1;
maxnagazumi 2:0be4cfbdf408 249 static int count=0;
maxnagazumi 1:a692014d8e41 250 switch(canx) {
maxnagazumi 1:a692014d8e41 251 case 1:
maxnagazumi 1:a692014d8e41 252 canOmega[0]=0;
maxnagazumi 2:0be4cfbdf408 253 canOmega[1]=a;
maxnagazumi 1:a692014d8e41 254 canOmega[2]=0;
maxnagazumi 2:0be4cfbdf408 255 canOmega[3]=-a;
maxnagazumi 2:0be4cfbdf408 256 printf(" 1");
maxnagazumi 1:a692014d8e41 257 break;
maxnagazumi 1:a692014d8e41 258 case 2:
maxnagazumi 2:0be4cfbdf408 259 canOmega[0]=a*1.4;
maxnagazumi 2:0be4cfbdf408 260 canOmega[1]=a*1.4;
maxnagazumi 2:0be4cfbdf408 261 canOmega[2]=-a*1.4;
maxnagazumi 2:0be4cfbdf408 262 canOmega[3]=-a*1.4;
maxnagazumi 2:0be4cfbdf408 263 printf(" 2");
maxnagazumi 1:a692014d8e41 264 break;
maxnagazumi 1:a692014d8e41 265 case 3:
maxnagazumi 2:0be4cfbdf408 266 canOmega[0]=a;
maxnagazumi 1:a692014d8e41 267 canOmega[1]=0;
maxnagazumi 2:0be4cfbdf408 268 canOmega[2]=-a;
maxnagazumi 1:a692014d8e41 269 canOmega[3]=0;
maxnagazumi 2:0be4cfbdf408 270 printf(" 3");
maxnagazumi 1:a692014d8e41 271 break;
maxnagazumi 1:a692014d8e41 272 case 4:
maxnagazumi 2:0be4cfbdf408 273 canOmega[0]=-a*1.4;
maxnagazumi 2:0be4cfbdf408 274 canOmega[1]=a*1.4;
maxnagazumi 2:0be4cfbdf408 275 canOmega[2]=a*1.4;
maxnagazumi 2:0be4cfbdf408 276 canOmega[3]=-a*1.4;
maxnagazumi 2:0be4cfbdf408 277 printf(" 4");
maxnagazumi 1:a692014d8e41 278 break;
maxnagazumi 1:a692014d8e41 279 case 5:
maxnagazumi 1:a692014d8e41 280 for(int i=0; i<4; i++) {
maxnagazumi 1:a692014d8e41 281 canOmega[i]=0;
maxnagazumi 1:a692014d8e41 282 }
maxnagazumi 2:0be4cfbdf408 283 printf(" 5");
maxnagazumi 1:a692014d8e41 284 break;
maxnagazumi 1:a692014d8e41 285 case 6:
maxnagazumi 2:0be4cfbdf408 286 canOmega[0]=a*1.4;
maxnagazumi 2:0be4cfbdf408 287 canOmega[1]=-a*1.4;
maxnagazumi 2:0be4cfbdf408 288 canOmega[2]=-a*1.4;
maxnagazumi 2:0be4cfbdf408 289 canOmega[3]=a*1.4;
maxnagazumi 2:0be4cfbdf408 290 printf(" 6");
maxnagazumi 1:a692014d8e41 291 break;
maxnagazumi 1:a692014d8e41 292 case 7:
maxnagazumi 2:0be4cfbdf408 293 canOmega[0]=0;
maxnagazumi 2:0be4cfbdf408 294 canOmega[1]=-a;
maxnagazumi 2:0be4cfbdf408 295 canOmega[2]=0;
maxnagazumi 2:0be4cfbdf408 296 canOmega[3]=a;
maxnagazumi 2:0be4cfbdf408 297 printf(" 7");
maxnagazumi 1:a692014d8e41 298 break;
maxnagazumi 1:a692014d8e41 299 case 8:
maxnagazumi 2:0be4cfbdf408 300 canOmega[0]=-a*1.4;
maxnagazumi 2:0be4cfbdf408 301 canOmega[1]=-a*1.4;
maxnagazumi 2:0be4cfbdf408 302 canOmega[2]=a*1.4;
maxnagazumi 2:0be4cfbdf408 303 canOmega[3]=a*1.4;
maxnagazumi 2:0be4cfbdf408 304 printf(" 8");
maxnagazumi 1:a692014d8e41 305 break;
maxnagazumi 1:a692014d8e41 306 case 9:
maxnagazumi 2:0be4cfbdf408 307 canOmega[0]=-a;
maxnagazumi 2:0be4cfbdf408 308 canOmega[1]=0;
maxnagazumi 2:0be4cfbdf408 309 canOmega[2]=a;
maxnagazumi 2:0be4cfbdf408 310 canOmega[3]=0;
maxnagazumi 2:0be4cfbdf408 311 printf(" 9");
maxnagazumi 1:a692014d8e41 312 break;
maxnagazumi 1:a692014d8e41 313 case 10:
maxnagazumi 1:a692014d8e41 314 for(int i=0; i<4; i++) {
maxnagazumi 2:0be4cfbdf408 315 canOmega[i]=a;
maxnagazumi 1:a692014d8e41 316 }
maxnagazumi 2:0be4cfbdf408 317 printf(" 10");
maxnagazumi 1:a692014d8e41 318 break;
maxnagazumi 1:a692014d8e41 319 case 11:
maxnagazumi 1:a692014d8e41 320 for(int i=0; i<4; i++) {
maxnagazumi 2:0be4cfbdf408 321 canOmega[i]=-a;
maxnagazumi 1:a692014d8e41 322 }
maxnagazumi 2:0be4cfbdf408 323 printf(" 11");
maxnagazumi 1:a692014d8e41 324 break;
maxnagazumi 1:a692014d8e41 325 case 12:
maxnagazumi 2:0be4cfbdf408 326 wait(0.2);
maxnagazumi 2:0be4cfbdf408 327 if(count==1) {
maxnagazumi 2:0be4cfbdf408 328 a=0.1;
maxnagazumi 2:0be4cfbdf408 329 count=0;
maxnagazumi 1:a692014d8e41 330 } else {
maxnagazumi 2:0be4cfbdf408 331 a=0.05;
maxnagazumi 2:0be4cfbdf408 332 count=1;
maxnagazumi 1:a692014d8e41 333 }
maxnagazumi 2:0be4cfbdf408 334 printf(" 12 Speed change");
maxnagazumi 1:a692014d8e41 335 break;
maxnagazumi 1:a692014d8e41 336
maxnagazumi 1:a692014d8e41 337 }
maxnagazumi 2:0be4cfbdf408 338 printf(" %f\r\n",a);
maxnagazumi 2:0be4cfbdf408 339 int i=0;
maxnagazumi 2:0be4cfbdf408 340 while(i<4) {
maxnagazumi 2:0be4cfbdf408 341 motor[i].turn(canOmega[i]);
maxnagazumi 2:0be4cfbdf408 342 i++;
maxnagazumi 1:a692014d8e41 343 }
maxnagazumi 1:a692014d8e41 344 }
maxnagazumi 1:a692014d8e41 345
maxnagazumi 1:a692014d8e41 346 //ticker に入れる関数
maxnagazumi 1:a692014d8e41 347 Ticker ticker;
maxnagazumi 1:a692014d8e41 348 Ticker canTicker;
maxnagazumi 1:a692014d8e41 349 Ticker locTicker;
maxnagazumi 0:c0e9bbc27454 350
maxnagazumi 0:c0e9bbc27454 351 //出力
maxnagazumi 0:c0e9bbc27454 352 void motorOut()
maxnagazumi 0:c0e9bbc27454 353 {
maxnagazumi 0:c0e9bbc27454 354 for(int i=0; i<4; i++) {
maxnagazumi 0:c0e9bbc27454 355 motor[i].Sc(omega.getOmega(i));
maxnagazumi 0:c0e9bbc27454 356 }
maxnagazumi 0:c0e9bbc27454 357 }
maxnagazumi 1:a692014d8e41 358 CAN_ticker canx;
maxnagazumi 1:a692014d8e41 359 void ticker_CanRead()
maxnagazumi 1:a692014d8e41 360 {
maxnagazumi 2:0be4cfbdf408 361 canx.canmsg_read();
maxnagazumi 2:0be4cfbdf408 362 /*for(int i=0; i<4; i++) {
maxnagazumi 2:0be4cfbdf408 363 motor[i].Sc(canOmega[i]);
maxnagazumi 2:0be4cfbdf408 364 }*/
maxnagazumi 1:a692014d8e41 365 }
maxnagazumi 1:a692014d8e41 366
maxnagazumi 1:a692014d8e41 367 Location location;
maxnagazumi 1:a692014d8e41 368 void calLoc()
maxnagazumi 1:a692014d8e41 369 {
maxnagazumi 1:a692014d8e41 370 theta=gyro.getAngle()-angle; //角度受け取り
maxnagazumi 1:a692014d8e41 371 location.calXY();
maxnagazumi 1:a692014d8e41 372 }
maxnagazumi 0:c0e9bbc27454 373 int main()
maxnagazumi 0:c0e9bbc27454 374 {
maxnagazumi 0:c0e9bbc27454 375 can1.frequency(1000000);
maxnagazumi 0:c0e9bbc27454 376 gyro.initialize(); //main関数の最初に一度だけ実行
maxnagazumi 0:c0e9bbc27454 377 gyro.acc_offset();
maxnagazumi 2:0be4cfbdf408 378 angle=gyro.getAngle();//角度初期化
maxnagazumi 0:c0e9bbc27454 379 double z;
maxnagazumi 0:c0e9bbc27454 380 printf("start\r\n");
maxnagazumi 0:c0e9bbc27454 381 motor[0].setEquation(0.008031,-0.022300,-0.008839,-0.016290);
maxnagazumi 0:c0e9bbc27454 382 motor[1].setEquation(0.008878,-0.016622,-0.009702,-0.015806);
maxnagazumi 0:c0e9bbc27454 383 motor[2].setEquation(0.008637,-0.016537,-0.009397,-0.012159);
maxnagazumi 0:c0e9bbc27454 384 motor[3].setEquation(0.008096,-0.014822,-0.008801,-0.016645);
maxnagazumi 0:c0e9bbc27454 385
maxnagazumi 0:c0e9bbc27454 386 motor[0].setDutyLimit(0.4);
maxnagazumi 0:c0e9bbc27454 387 motor[1].setDutyLimit(0.4);
maxnagazumi 0:c0e9bbc27454 388 motor[2].setDutyLimit(0.4);
maxnagazumi 0:c0e9bbc27454 389 motor[3].setDutyLimit(0.4);
maxnagazumi 0:c0e9bbc27454 390
maxnagazumi 2:0be4cfbdf408 391 motor[0].setPDparam( 0.01790, 0.00560);
maxnagazumi 2:0be4cfbdf408 392 motor[1].setPDparam( 0.01705, 0.00620);
maxnagazumi 2:0be4cfbdf408 393 motor[2].setPDparam( 0.01790, 0.00620);
maxnagazumi 2:0be4cfbdf408 394 motor[3].setPDparam( 0.01680, 0.00560);
maxnagazumi 0:c0e9bbc27454 395
maxnagazumi 1:a692014d8e41 396 int first=0;
maxnagazumi 1:a692014d8e41 397 while(first==0) {
maxnagazumi 0:c0e9bbc27454 398 printf("waiting\r\n");
maxnagazumi 0:c0e9bbc27454 399 if(button==0) {
maxnagazumi 0:c0e9bbc27454 400 wait(1);
maxnagazumi 1:a692014d8e41 401 ticker.attach(&motorOut,0.05);
maxnagazumi 1:a692014d8e41 402 first++;
maxnagazumi 0:c0e9bbc27454 403 break;
maxnagazumi 0:c0e9bbc27454 404 }
maxnagazumi 0:c0e9bbc27454 405 }
maxnagazumi 0:c0e9bbc27454 406
maxnagazumi 1:a692014d8e41 407 int canX=20;//can変数
maxnagazumi 0:c0e9bbc27454 408 int n=1,dx,dy,aimX,aimY;
maxnagazumi 0:c0e9bbc27454 409 double vx_,vy_,vx,vy,newtime,distance;
maxnagazumi 0:c0e9bbc27454 410 Timer time;
maxnagazumi 0:c0e9bbc27454 411 time.start();
maxnagazumi 1:a692014d8e41 412 locTicker.attach(&calLoc,0.05);
maxnagazumi 1:a692014d8e41 413 canTicker.attach(&ticker_CanRead,0.1);//can読み込み
maxnagazumi 0:c0e9bbc27454 414 while(1) {
maxnagazumi 2:0be4cfbdf408 415 canX=canx.get_xCAN();//０で手\動化
maxnagazumi 2:0be4cfbdf408 416 if(canX==13) {//手動化
maxnagazumi 2:0be4cfbdf408 417 wait(0.2);
maxnagazumi 2:0be4cfbdf408 418 printf("change1\r\n");
maxnagazumi 1:a692014d8e41 419 ticker.detach();
maxnagazumi 1:a692014d8e41 420 while(1) {
maxnagazumi 1:a692014d8e41 421 canX=canx.get_xCAN();
maxnagazumi 1:a692014d8e41 422 calOmega_CAN(canX);
maxnagazumi 1:a692014d8e41 423 //自己位置取得
maxnagazumi 1:a692014d8e41 424 x=location.getX();
maxnagazumi 1:a692014d8e41 425 y=location.getY();
maxnagazumi 2:0be4cfbdf408 426 printf("X=%06d ,Y=%06d",x,y);
maxnagazumi 1:a692014d8e41 427 //目的地決定(syuusoku check)
maxnagazumi 1:a692014d8e41 428 aimX = plot[n][0];
maxnagazumi 1:a692014d8e41 429 aimY = plot[n][1];
maxnagazumi 1:a692014d8e41 430 dx=aimX-x;
maxnagazumi 1:a692014d8e41 431 dy=aimY-y;
maxnagazumi 1:a692014d8e41 432 distance = sqrt((float)dx*dx+dy*dy);
maxnagazumi 2:0be4cfbdf408 433 if(distance<1000) {
maxnagazumi 1:a692014d8e41 434 n++;
maxnagazumi 1:a692014d8e41 435 printf("reach%d\r\n",n);
maxnagazumi 1:a692014d8e41 436 time.reset();
maxnagazumi 1:a692014d8e41 437 }
maxnagazumi 2:0be4cfbdf408 438 if(canX==13) {//自動化
maxnagazumi 1:a692014d8e41 439 canX=20;
maxnagazumi 2:0be4cfbdf408 440 for(int i=0; i<4; i++) {
maxnagazumi 2:0be4cfbdf408 441 canOmega[i]=0;
maxnagazumi 2:0be4cfbdf408 442 }
maxnagazumi 1:a692014d8e41 443 ticker.attach(&motorOut,0.05);
maxnagazumi 2:0be4cfbdf408 444 printf("change2\r\n");
maxnagazumi 2:0be4cfbdf408 445 wait(0.1);
maxnagazumi 1:a692014d8e41 446 break;
maxnagazumi 1:a692014d8e41 447 }
maxnagazumi 1:a692014d8e41 448 }
maxnagazumi 1:a692014d8e41 449 }
maxnagazumi 0:c0e9bbc27454 450 //自己位置取得
maxnagazumi 0:c0e9bbc27454 451 x=location.getX();
maxnagazumi 0:c0e9bbc27454 452 y=location.getY();
maxnagazumi 2:0be4cfbdf408 453 int i=0;
maxnagazumi 2:0be4cfbdf408 454 if(i==10) {
maxnagazumi 2:0be4cfbdf408 455 printf("X=%d,Y=%d,theta=%5.3f ,angle=%5.3f z=%5.3f n=%d\r\n",x,y,theta,angle,z,n);
maxnagazumi 2:0be4cfbdf408 456 for(int j=0; j<4; j++) {
maxnagazumi 2:0be4cfbdf408 457 printf("%d : %d,",j,ec[j].getCount());
maxnagazumi 2:0be4cfbdf408 458 }
maxnagazumi 2:0be4cfbdf408 459 printf("\r\n");
maxnagazumi 2:0be4cfbdf408 460 i=10;
maxnagazumi 2:0be4cfbdf408 461 } else {
maxnagazumi 2:0be4cfbdf408 462 i++;
maxnagazumi 2:0be4cfbdf408 463 }
maxnagazumi 0:c0e9bbc27454 464 //目的地決定(syuusoku check)
maxnagazumi 0:c0e9bbc27454 465 aimX = plot[n][0];
maxnagazumi 0:c0e9bbc27454 466 aimY = plot[n][1];
maxnagazumi 0:c0e9bbc27454 467 //出力を計算(kitai xy);
maxnagazumi 0:c0e9bbc27454 468 dx=aimX-x;
maxnagazumi 0:c0e9bbc27454 469 dy=aimY-y;
maxnagazumi 0:c0e9bbc27454 470 vx_=dx/sqrt((double)dx*dx+dy*dy);
maxnagazumi 0:c0e9bbc27454 471 vy_=dy/sqrt((double)dx*dx+dy*dy);
maxnagazumi 0:c0e9bbc27454 472 vx=vx_*cos(theta/180*3.14)+vy_*sin(theta/180*3.14);
maxnagazumi 0:c0e9bbc27454 473 vy=-vx_*sin(theta/180*3.14)+vy_*cos(theta/180*3.14);
maxnagazumi 0:c0e9bbc27454 474 //四輪の出力計算
maxnagazumi 0:c0e9bbc27454 475 newtime=time.read();
maxnagazumi 0:c0e9bbc27454 476 distance = sqrt((float)dx*dx+dy*dy);
maxnagazumi 0:c0e9bbc27454 477 z=pControl(distance,zMin[n],newtime);
maxnagazumi 0:c0e9bbc27454 478 omega.setOmega(z,0.05);
maxnagazumi 0:c0e9bbc27454 479 omega.setVxy(vx,vy,aimTheta[n]);
maxnagazumi 0:c0e9bbc27454 480 omega.calOmega();
maxnagazumi 0:c0e9bbc27454 481 //ゴール判定
maxnagazumi 0:c0e9bbc27454 482 if(distance<800) {
maxnagazumi 0:c0e9bbc27454 483 n++;
maxnagazumi 0:c0e9bbc27454 484 printf("reach%d\r\n",n);
maxnagazumi 0:c0e9bbc27454 485 time.reset();
maxnagazumi 0:c0e9bbc27454 486 }
maxnagazumi 0:c0e9bbc27454 487
maxnagazumi 2:0be4cfbdf408 488 if(n>=8) {
maxnagazumi 0:c0e9bbc27454 489 for(int j=0; j<4; j++) {
maxnagazumi 1:a692014d8e41 490 motor[j].stop();
maxnagazumi 0:c0e9bbc27454 491 }
maxnagazumi 0:c0e9bbc27454 492 printf("fin\r\n");
maxnagazumi 0:c0e9bbc27454 493 ticker.detach();
maxnagazumi 0:c0e9bbc27454 494 }
maxnagazumi 0:c0e9bbc27454 495 }
maxnagazumi 0:c0e9bbc27454 496 }