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

大会本番用

Dependencies:   mbed SpeedController hcsr04 Encoder CruizCore_R1370P

main.cpp@1:a692014d8e41, 2020-03-12 (annotated)

Committer:
maxnagazumi
Date:
Thu Mar 12 05:52:39 2020 +0000
Revision:
1:a692014d8e41
Parent:
0:c0e9bbc27454
Child:
2:0be4cfbdf408
aa

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 1:a692014d8e41 11 0 start
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 0:c0e9bbc27454 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 0:c0e9bbc27454 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 0:c0e9bbc27454 100 ,{800,13500}
maxnagazumi 0:c0e9bbc27454 101 ,{1000,15500}
maxnagazumi 0:c0e9bbc27454 102 ,{8654,16500}
maxnagazumi 0:c0e9bbc27454 103 ,{16000,16500}
maxnagazumi 0:c0e9bbc27454 104 };
maxnagazumi 0:c0e9bbc27454 105
maxnagazumi 0:c0e9bbc27454 106 double aimTheta[]= {//目標角度を指定
maxnagazumi 0:c0e9bbc27454 107 0,0,0,0,0,0,0,0,0,0,0,0
maxnagazumi 0:c0e9bbc27454 108 };
maxnagazumi 0:c0e9bbc27454 109
maxnagazumi 0:c0e9bbc27454 110 double zMin[]= { //速度の最少を指定
maxnagazumi 0:c0e9bbc27454 111 2,2,2, 2,2,2, 5,5,5, 5,5,5
maxnagazumi 0:c0e9bbc27454 112 };
maxnagazumi 0:c0e9bbc27454 113
maxnagazumi 0:c0e9bbc27454 114 //出力を計算
maxnagazumi 0:c0e9bbc27454 115 int x,y;
maxnagazumi 0:c0e9bbc27454 116
maxnagazumi 0:c0e9bbc27454 117 class WheelOmega
maxnagazumi 0:c0e9bbc27454 118 {
maxnagazumi 0:c0e9bbc27454 119 public:
maxnagazumi 0:c0e9bbc27454 120 WheelOmega(): max_(0),vx_(0),vy_(0),theta_(0)
maxnagazumi 0:c0e9bbc27454 121 {
maxnagazumi 0:c0e9bbc27454 122 for(int i=0; i<4; i++) {
maxnagazumi 0:c0e9bbc27454 123 omega[i]=0;
maxnagazumi 0:c0e9bbc27454 124 }
maxnagazumi 0:c0e9bbc27454 125 }
maxnagazumi 0:c0e9bbc27454 126 void setOmega(double max,double k)
maxnagazumi 0:c0e9bbc27454 127 {
maxnagazumi 0:c0e9bbc27454 128 max_=max;
maxnagazumi 0:c0e9bbc27454 129 k_=k;
maxnagazumi 0:c0e9bbc27454 130
maxnagazumi 0:c0e9bbc27454 131 }
maxnagazumi 0:c0e9bbc27454 132 void setVxy(double vx,double vy,double aimtheta_)
maxnagazumi 0:c0e9bbc27454 133 {
maxnagazumi 0:c0e9bbc27454 134 vx_=vx;
maxnagazumi 0:c0e9bbc27454 135 vy_=vy;
maxnagazumi 0:c0e9bbc27454 136 theta_=aimtheta_ - theta;
maxnagazumi 0:c0e9bbc27454 137 if(theta_>30) {//目標角度まで３０度以上空いていたら補正、係数調整のため３０は適当
maxnagazumi 0:c0e9bbc27454 138 theta_=30;
maxnagazumi 0:c0e9bbc27454 139 }
maxnagazumi 0:c0e9bbc27454 140 if(theta_<-30) {
maxnagazumi 0:c0e9bbc27454 141 theta_=-30;
maxnagazumi 0:c0e9bbc27454 142 }
maxnagazumi 0:c0e9bbc27454 143 }
maxnagazumi 0:c0e9bbc27454 144 void calOmega()
maxnagazumi 0:c0e9bbc27454 145 {
maxnagazumi 0:c0e9bbc27454 146 double theta_rad=45/180*3.14;
maxnagazumi 0:c0e9bbc27454 147 omega[0]=max_*vx_*cos(theta_rad)-max_*vy_*cos(theta_rad) + theta_*k_;
maxnagazumi 0:c0e9bbc27454 148 omega[1]=-max_*vx_*cos(theta_rad)-max_*vy_*cos(theta_rad)+theta_*k_;
maxnagazumi 0:c0e9bbc27454 149 omega[2]=-max_*vx_*cos(theta_rad)+max_*vy_*cos(theta_rad)+theta_*k_;
maxnagazumi 0:c0e9bbc27454 150 omega[3]=max_*vx_*cos(theta_rad)+max_*vy_*cos(theta_rad)+theta_*k_;
maxnagazumi 0:c0e9bbc27454 151 };
maxnagazumi 0:c0e9bbc27454 152 double getOmega(int i)
maxnagazumi 0:c0e9bbc27454 153 {
maxnagazumi 0:c0e9bbc27454 154 return omega[i];
maxnagazumi 0:c0e9bbc27454 155 }
maxnagazumi 0:c0e9bbc27454 156 private:
maxnagazumi 0:c0e9bbc27454 157 double max_,vx_,vy_,theta_,k_;
maxnagazumi 0:c0e9bbc27454 158 double omega[4];
maxnagazumi 0:c0e9bbc27454 159 };
maxnagazumi 0:c0e9bbc27454 160
maxnagazumi 0:c0e9bbc27454 161 WheelOmega omega;
maxnagazumi 0:c0e9bbc27454 162 //パラメタ処理
maxnagazumi 0:c0e9bbc27454 163 double pControl(double distance_,double zMin,double newtime)
maxnagazumi 0:c0e9bbc27454 164 {
maxnagazumi 0:c0e9bbc27454 165 double z,zMax,olddistance,oldtime;
maxnagazumi 0:c0e9bbc27454 166 double diftime_;
maxnagazumi 0:c0e9bbc27454 167 diftime_ = newtime - oldtime;
maxnagazumi 0:c0e9bbc27454 168 oldtime= newtime;
maxnagazumi 0:c0e9bbc27454 169 z=0.004*distance_ - 0.1*(olddistance-distance_)/diftime_;
maxnagazumi 0:c0e9bbc27454 170 zMax=2;
maxnagazumi 0:c0e9bbc27454 171 if(z>zMax) {
maxnagazumi 0:c0e9bbc27454 172 z=zMax;
maxnagazumi 0:c0e9bbc27454 173 }
maxnagazumi 0:c0e9bbc27454 174 if(z<zMin) {
maxnagazumi 0:c0e9bbc27454 175 z=zMin;
maxnagazumi 0:c0e9bbc27454 176 }
maxnagazumi 0:c0e9bbc27454 177 if(newtime<1) {
maxnagazumi 0:c0e9bbc27454 178 z=z*newtime;
maxnagazumi 0:c0e9bbc27454 179 }
maxnagazumi 0:c0e9bbc27454 180 olddistance = distance_;
maxnagazumi 0:c0e9bbc27454 181 return z;
maxnagazumi 0:c0e9bbc27454 182 }
maxnagazumi 0:c0e9bbc27454 183
maxnagazumi 0:c0e9bbc27454 184 //超音波
maxnagazumi 0:c0e9bbc27454 185 class Sonic
maxnagazumi 0:c0e9bbc27454 186 {
maxnagazumi 1:a692014d8e41 187 public:
maxnagazumi 0:c0e9bbc27454 188 Sonic()
maxnagazumi 0:c0e9bbc27454 189 {
maxnagazumi 0:c0e9bbc27454 190 for(int i=0; i<4; i++) {
maxnagazumi 0:c0e9bbc27454 191 sonic_cm[i]=0;
maxnagazumi 0:c0e9bbc27454 192 }
maxnagazumi 0:c0e9bbc27454 193 }
maxnagazumi 0:c0e9bbc27454 194
maxnagazumi 0:c0e9bbc27454 195 void cal_sonic()
maxnagazumi 0:c0e9bbc27454 196 {
maxnagazumi 0:c0e9bbc27454 197 for(int i=0; i<4; i++) {
maxnagazumi 0:c0e9bbc27454 198 echo[i].start();
maxnagazumi 0:c0e9bbc27454 199 }
maxnagazumi 0:c0e9bbc27454 200 for(int i=0; i<4; i++) {
maxnagazumi 0:c0e9bbc27454 201 sonic_cm[i] =echo[i].get_dist_cm();
maxnagazumi 0:c0e9bbc27454 202 }
maxnagazumi 0:c0e9bbc27454 203 }
maxnagazumi 0:c0e9bbc27454 204 double get_sonic(int i)
maxnagazumi 0:c0e9bbc27454 205 {
maxnagazumi 0:c0e9bbc27454 206 return sonic_cm[i];
maxnagazumi 0:c0e9bbc27454 207 }
maxnagazumi 0:c0e9bbc27454 208
maxnagazumi 0:c0e9bbc27454 209 private:
maxnagazumi 0:c0e9bbc27454 210 double sonic_cm[4];
maxnagazumi 0:c0e9bbc27454 211 };
maxnagazumi 0:c0e9bbc27454 212
maxnagazumi 1:a692014d8e41 213 class CAN_ticker
maxnagazumi 1:a692014d8e41 214 {
maxnagazumi 1:a692014d8e41 215 public:
maxnagazumi 1:a692014d8e41 216 CAN_ticker():x(0),data(0) {}
maxnagazumi 1:a692014d8e41 217 void can_read()
maxnagazumi 1:a692014d8e41 218 {
maxnagazumi 1:a692014d8e41 219 CANMessage msg;
maxnagazumi 1:a692014d8e41 220 if(can1.read(msg)) {
maxnagazumi 1:a692014d8e41 221 if(msg.id == 1) {
maxnagazumi 1:a692014d8e41 222 x=(short)(msg.data);
maxnagazumi 1:a692014d8e41 223 }
maxnagazumi 1:a692014d8e41 224 }
maxnagazumi 1:a692014d8e41 225 }
maxnagazumi 1:a692014d8e41 226 int get_xCAN()
maxnagazumi 1:a692014d8e41 227 {
maxnagazumi 1:a692014d8e41 228 return x;
maxnagazumi 1:a692014d8e41 229 }
maxnagazumi 1:a692014d8e41 230 private:
maxnagazumi 1:a692014d8e41 231 char data;
maxnagazumi 1:a692014d8e41 232 int x;
maxnagazumi 1:a692014d8e41 233 };
maxnagazumi 1:a692014d8e41 234 //手動出力
maxnagazumi 1:a692014d8e41 235 void calOmega_CAN(int canx)
maxnagazumi 1:a692014d8e41 236 {
maxnagazumi 1:a692014d8e41 237 int a=10,count=0;
maxnagazumi 1:a692014d8e41 238 double canOmega[4];
maxnagazumi 1:a692014d8e41 239 switch(canx) {
maxnagazumi 1:a692014d8e41 240 case 1:
maxnagazumi 1:a692014d8e41 241 canOmega[0]=0;
maxnagazumi 1:a692014d8e41 242 canOmega[1]=-a;
maxnagazumi 1:a692014d8e41 243 canOmega[2]=0;
maxnagazumi 1:a692014d8e41 244 canOmega[3]=a;
maxnagazumi 1:a692014d8e41 245 break;
maxnagazumi 1:a692014d8e41 246 case 2:
maxnagazumi 1:a692014d8e41 247 canOmega[0]=-a*1.4;
maxnagazumi 1:a692014d8e41 248 canOmega[1]=-a*1.4;
maxnagazumi 1:a692014d8e41 249 canOmega[2]=a*1.4;
maxnagazumi 1:a692014d8e41 250 canOmega[3]=a*1.4;
maxnagazumi 1:a692014d8e41 251 break;
maxnagazumi 1:a692014d8e41 252 case 3:
maxnagazumi 1:a692014d8e41 253 canOmega[0]=-a;
maxnagazumi 1:a692014d8e41 254 canOmega[1]=0;
maxnagazumi 1:a692014d8e41 255 canOmega[2]=a;
maxnagazumi 1:a692014d8e41 256 canOmega[3]=0;
maxnagazumi 1:a692014d8e41 257 break;
maxnagazumi 1:a692014d8e41 258 case 4:
maxnagazumi 1:a692014d8e41 259 canOmega[0]=a*1.4;
maxnagazumi 1:a692014d8e41 260 canOmega[1]=-a*1.4;
maxnagazumi 1:a692014d8e41 261 canOmega[2]=-a*1.4;
maxnagazumi 1:a692014d8e41 262 canOmega[3]=a*1.4;
maxnagazumi 1:a692014d8e41 263 break;
maxnagazumi 1:a692014d8e41 264 case 5:
maxnagazumi 1:a692014d8e41 265 for(int i=0; i<4; i++) {
maxnagazumi 1:a692014d8e41 266 canOmega[i]=0;
maxnagazumi 1:a692014d8e41 267 }
maxnagazumi 1:a692014d8e41 268 break;
maxnagazumi 1:a692014d8e41 269 case 6:
maxnagazumi 1:a692014d8e41 270 canOmega[0]=-a*1.4;
maxnagazumi 1:a692014d8e41 271 canOmega[1]=a*1.4;
maxnagazumi 1:a692014d8e41 272 canOmega[2]=a*1.4;
maxnagazumi 1:a692014d8e41 273 canOmega[3]=-a*1.4;
maxnagazumi 1:a692014d8e41 274 break;
maxnagazumi 1:a692014d8e41 275 case 7:
maxnagazumi 1:a692014d8e41 276 canOmega[0]=a;
maxnagazumi 1:a692014d8e41 277 canOmega[1]=0;
maxnagazumi 1:a692014d8e41 278 canOmega[2]=-a;
maxnagazumi 1:a692014d8e41 279 canOmega[3]=0;
maxnagazumi 1:a692014d8e41 280 break;
maxnagazumi 1:a692014d8e41 281 case 8:
maxnagazumi 1:a692014d8e41 282 canOmega[0]=a*1.4;
maxnagazumi 1:a692014d8e41 283 canOmega[1]=a*1.4;
maxnagazumi 1:a692014d8e41 284 canOmega[2]=-a*1.4;
maxnagazumi 1:a692014d8e41 285 canOmega[3]=-a*1.4;
maxnagazumi 1:a692014d8e41 286 break;
maxnagazumi 1:a692014d8e41 287 case 9:
maxnagazumi 1:a692014d8e41 288 canOmega[0]=0;
maxnagazumi 1:a692014d8e41 289 canOmega[1]=a;
maxnagazumi 1:a692014d8e41 290 canOmega[2]=0;
maxnagazumi 1:a692014d8e41 291 canOmega[3]=-a;
maxnagazumi 1:a692014d8e41 292 break;
maxnagazumi 1:a692014d8e41 293 case 10:
maxnagazumi 1:a692014d8e41 294 for(int i=0; i<4; i++) {
maxnagazumi 1:a692014d8e41 295 canOmega[i]=-a;
maxnagazumi 1:a692014d8e41 296 }
maxnagazumi 1:a692014d8e41 297 break;
maxnagazumi 1:a692014d8e41 298 case 11:
maxnagazumi 1:a692014d8e41 299 for(int i=0; i<4; i++) {
maxnagazumi 1:a692014d8e41 300 canOmega[i]=a;
maxnagazumi 1:a692014d8e41 301 }
maxnagazumi 1:a692014d8e41 302 break;
maxnagazumi 1:a692014d8e41 303 case 12:
maxnagazumi 1:a692014d8e41 304 if(count%2==1) {
maxnagazumi 1:a692014d8e41 305 a=20;
maxnagazumi 1:a692014d8e41 306 } else {
maxnagazumi 1:a692014d8e41 307 a=10;
maxnagazumi 1:a692014d8e41 308 }
maxnagazumi 1:a692014d8e41 309 break;
maxnagazumi 1:a692014d8e41 310
maxnagazumi 1:a692014d8e41 311
maxnagazumi 1:a692014d8e41 312 }
maxnagazumi 1:a692014d8e41 313 for(int i=0; i<4; i++) {
maxnagazumi 1:a692014d8e41 314 motor[i].Sc(canOmega[i]);
maxnagazumi 1:a692014d8e41 315 }
maxnagazumi 1:a692014d8e41 316
maxnagazumi 1:a692014d8e41 317 }
maxnagazumi 1:a692014d8e41 318
maxnagazumi 1:a692014d8e41 319 //ticker に入れる関数
maxnagazumi 1:a692014d8e41 320 Ticker ticker;
maxnagazumi 1:a692014d8e41 321 Ticker canTicker;
maxnagazumi 1:a692014d8e41 322 Ticker locTicker;
maxnagazumi 0:c0e9bbc27454 323
maxnagazumi 0:c0e9bbc27454 324 //出力
maxnagazumi 0:c0e9bbc27454 325 void motorOut()
maxnagazumi 0:c0e9bbc27454 326 {
maxnagazumi 0:c0e9bbc27454 327 for(int i=0; i<4; i++) {
maxnagazumi 0:c0e9bbc27454 328 motor[i].Sc(omega.getOmega(i));
maxnagazumi 0:c0e9bbc27454 329 }
maxnagazumi 0:c0e9bbc27454 330 }
maxnagazumi 0:c0e9bbc27454 331
maxnagazumi 1:a692014d8e41 332 CAN_ticker canx;
maxnagazumi 1:a692014d8e41 333 void ticker_CanRead()
maxnagazumi 1:a692014d8e41 334 {
maxnagazumi 1:a692014d8e41 335 canx.can_read();
maxnagazumi 1:a692014d8e41 336 }
maxnagazumi 1:a692014d8e41 337
maxnagazumi 1:a692014d8e41 338 Location location;
maxnagazumi 1:a692014d8e41 339 void calLoc()
maxnagazumi 1:a692014d8e41 340 {
maxnagazumi 1:a692014d8e41 341 theta=gyro.getAngle()-angle; //角度受け取り
maxnagazumi 1:a692014d8e41 342 location.calXY();
maxnagazumi 1:a692014d8e41 343 }
maxnagazumi 0:c0e9bbc27454 344 int main()
maxnagazumi 0:c0e9bbc27454 345 {
maxnagazumi 1:a692014d8e41 346 angle=gyro.getAngle();//角度初期化
maxnagazumi 0:c0e9bbc27454 347 can1.frequency(1000000);
maxnagazumi 0:c0e9bbc27454 348 gyro.initialize(); //main関数の最初に一度だけ実行
maxnagazumi 0:c0e9bbc27454 349 gyro.acc_offset();
maxnagazumi 0:c0e9bbc27454 350 double z;
maxnagazumi 0:c0e9bbc27454 351 printf("start\r\n");
maxnagazumi 0:c0e9bbc27454 352 motor[0].setEquation(0.008031,-0.022300,-0.008839,-0.016290);
maxnagazumi 0:c0e9bbc27454 353 motor[1].setEquation(0.008878,-0.016622,-0.009702,-0.015806);
maxnagazumi 0:c0e9bbc27454 354 motor[2].setEquation(0.008637,-0.016537,-0.009397,-0.012159);
maxnagazumi 0:c0e9bbc27454 355 motor[3].setEquation(0.008096,-0.014822,-0.008801,-0.016645);
maxnagazumi 0:c0e9bbc27454 356
maxnagazumi 0:c0e9bbc27454 357 motor[0].setDutyLimit(0.4);
maxnagazumi 0:c0e9bbc27454 358 motor[1].setDutyLimit(0.4);
maxnagazumi 0:c0e9bbc27454 359 motor[2].setDutyLimit(0.4);
maxnagazumi 0:c0e9bbc27454 360 motor[3].setDutyLimit(0.4);
maxnagazumi 0:c0e9bbc27454 361
maxnagazumi 0:c0e9bbc27454 362 motor[0].setPDparam( 0.1790, 0.00560);
maxnagazumi 0:c0e9bbc27454 363 motor[1].setPDparam( 0.1705, 0.00620);
maxnagazumi 0:c0e9bbc27454 364 motor[2].setPDparam( 0.1790, 0.00620);
maxnagazumi 0:c0e9bbc27454 365 motor[3].setPDparam( 0.1680, 0.00560);
maxnagazumi 0:c0e9bbc27454 366
maxnagazumi 1:a692014d8e41 367 int first=0;
maxnagazumi 1:a692014d8e41 368 while(first==0) {
maxnagazumi 0:c0e9bbc27454 369 printf("waiting\r\n");
maxnagazumi 0:c0e9bbc27454 370 if(button==0) {
maxnagazumi 0:c0e9bbc27454 371 wait(1);
maxnagazumi 1:a692014d8e41 372 ticker.attach(&motorOut,0.05);
maxnagazumi 1:a692014d8e41 373 first++;
maxnagazumi 0:c0e9bbc27454 374 break;
maxnagazumi 0:c0e9bbc27454 375 }
maxnagazumi 0:c0e9bbc27454 376 }
maxnagazumi 0:c0e9bbc27454 377
maxnagazumi 1:a692014d8e41 378 int canX=20;//can変数
maxnagazumi 0:c0e9bbc27454 379 int n=1,dx,dy,aimX,aimY;
maxnagazumi 0:c0e9bbc27454 380 double vx_,vy_,vx,vy,newtime,distance;
maxnagazumi 0:c0e9bbc27454 381 Timer time;
maxnagazumi 0:c0e9bbc27454 382 time.start();
maxnagazumi 1:a692014d8e41 383 locTicker.attach(&calLoc,0.05);
maxnagazumi 1:a692014d8e41 384 canTicker.attach(&ticker_CanRead,0.1);//can読み込み
maxnagazumi 0:c0e9bbc27454 385 while(1) {
maxnagazumi 1:a692014d8e41 386 canX=canx.get_xCAN();//０で手動化
maxnagazumi 1:a692014d8e41 387 if(canX==0) {//手動化
maxnagazumi 1:a692014d8e41 388 ticker.detach();
maxnagazumi 1:a692014d8e41 389 while(1) {
maxnagazumi 1:a692014d8e41 390 canX=canx.get_xCAN();
maxnagazumi 1:a692014d8e41 391 calOmega_CAN(canX);
maxnagazumi 1:a692014d8e41 392 //自己位置取得
maxnagazumi 1:a692014d8e41 393 x=location.getX();
maxnagazumi 1:a692014d8e41 394 y=location.getY();
maxnagazumi 1:a692014d8e41 395 //目的地決定(syuusoku check)
maxnagazumi 1:a692014d8e41 396 aimX = plot[n][0];
maxnagazumi 1:a692014d8e41 397 aimY = plot[n][1];
maxnagazumi 1:a692014d8e41 398 dx=aimX-x;
maxnagazumi 1:a692014d8e41 399 dy=aimY-y;
maxnagazumi 1:a692014d8e41 400 distance = sqrt((float)dx*dx+dy*dy);
maxnagazumi 1:a692014d8e41 401 if(distance<800) {
maxnagazumi 1:a692014d8e41 402 n++;
maxnagazumi 1:a692014d8e41 403 printf("reach%d\r\n",n);
maxnagazumi 1:a692014d8e41 404 time.reset();
maxnagazumi 1:a692014d8e41 405 }
maxnagazumi 1:a692014d8e41 406 if(canX==0) {//自動化
maxnagazumi 1:a692014d8e41 407 canX=20;
maxnagazumi 1:a692014d8e41 408 ticker.attach(&motorOut,0.05);
maxnagazumi 1:a692014d8e41 409 break;
maxnagazumi 1:a692014d8e41 410 }
maxnagazumi 1:a692014d8e41 411 }
maxnagazumi 1:a692014d8e41 412 }
maxnagazumi 0:c0e9bbc27454 413 //自己位置取得
maxnagazumi 0:c0e9bbc27454 414 x=location.getX();
maxnagazumi 0:c0e9bbc27454 415 y=location.getY();
maxnagazumi 0:c0e9bbc27454 416 printf("X=%d,Y=%d,theta=%5.3f z=%5.3f %f\r\n",x,y,theta,z,time.read());
maxnagazumi 0:c0e9bbc27454 417
maxnagazumi 0:c0e9bbc27454 418 //目的地決定(syuusoku check)
maxnagazumi 0:c0e9bbc27454 419 aimX = plot[n][0];
maxnagazumi 0:c0e9bbc27454 420 aimY = plot[n][1];
maxnagazumi 0:c0e9bbc27454 421 //出力を計算(kitai xy);
maxnagazumi 0:c0e9bbc27454 422 dx=aimX-x;
maxnagazumi 0:c0e9bbc27454 423 dy=aimY-y;
maxnagazumi 0:c0e9bbc27454 424 vx_=dx/sqrt((double)dx*dx+dy*dy);
maxnagazumi 0:c0e9bbc27454 425 vy_=dy/sqrt((double)dx*dx+dy*dy);
maxnagazumi 0:c0e9bbc27454 426 vx=vx_*cos(theta/180*3.14)+vy_*sin(theta/180*3.14);
maxnagazumi 0:c0e9bbc27454 427 vy=-vx_*sin(theta/180*3.14)+vy_*cos(theta/180*3.14);
maxnagazumi 0:c0e9bbc27454 428 //四輪の出力計算
maxnagazumi 0:c0e9bbc27454 429 newtime=time.read();
maxnagazumi 0:c0e9bbc27454 430 distance = sqrt((float)dx*dx+dy*dy);
maxnagazumi 0:c0e9bbc27454 431 z=pControl(distance,zMin[n],newtime);
maxnagazumi 0:c0e9bbc27454 432 omega.setOmega(z,0.05);
maxnagazumi 0:c0e9bbc27454 433 omega.setVxy(vx,vy,aimTheta[n]);
maxnagazumi 0:c0e9bbc27454 434 omega.calOmega();
maxnagazumi 0:c0e9bbc27454 435 //ゴール判定
maxnagazumi 0:c0e9bbc27454 436 if(distance<800) {
maxnagazumi 0:c0e9bbc27454 437 n++;
maxnagazumi 0:c0e9bbc27454 438 printf("reach%d\r\n",n);
maxnagazumi 0:c0e9bbc27454 439 time.reset();
maxnagazumi 0:c0e9bbc27454 440 }
maxnagazumi 0:c0e9bbc27454 441
maxnagazumi 0:c0e9bbc27454 442 if(n>=5) {
maxnagazumi 0:c0e9bbc27454 443 for(int j=0; j<4; j++) {
maxnagazumi 1:a692014d8e41 444 motor[j].stop();
maxnagazumi 0:c0e9bbc27454 445 }
maxnagazumi 0:c0e9bbc27454 446 printf("fin\r\n");
maxnagazumi 0:c0e9bbc27454 447 ticker.detach();
maxnagazumi 0:c0e9bbc27454 448 }
maxnagazumi 0:c0e9bbc27454 449 }
maxnagazumi 0:c0e9bbc27454 450 }