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

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed SpeedController Encoder CruizCore_R1370P

main.cpp@20:acb13e232678, 2020-03-09 (annotated)

Committer:
maxnagazumi
Date:
Mon Mar 09 08:35:41 2020 +0000
Revision:
20:acb13e232678
Parent:
19:0c1c3b2e009f
Child:
21:46b85fee5e88
3/9

Who changed what in which revision?

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