春ロボ　ロケット団 / 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@22:5c78d75fb2f2, 2020-03-09 (annotated)

Committer:
maxnagazumi
Date:
Mon Mar 09 11:56:16 2020 +0000
Revision:
22:5c78d75fb2f2
Parent:
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 22:5c78d75fb2f2 87 0,-30,-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 22:5c78d75fb2f2 120 double theta_rad=45/180*3.14;
maxnagazumi 22:5c78d75fb2f2 121 omega[0]=max_*vx_*cos(theta_rad)-max_*vy_*cos(theta_rad) + theta_*k_;
maxnagazumi 22:5c78d75fb2f2 122 omega[1]=-max_*vx_*cos(theta_rad)-max_*vy_*cos(theta_rad)+theta_*k_;
maxnagazumi 22:5c78d75fb2f2 123 omega[2]=-max_*vx_*cos(theta_rad)+max_*vy_*cos(theta_rad)+theta_*k_;
maxnagazumi 22:5c78d75fb2f2 124 omega[3]=max_*vx_*cos(theta_rad)+max_*vy_*cos(theta_rad)+theta_*k_;
maxnagazumi 3:8b9ba783512e 125 };
maxnagazumi 3:8b9ba783512e 126 double getOmega(int i)
maxnagazumi 3:8b9ba783512e 127 {
maxnagazumi 3:8b9ba783512e 128 return omega[i];
maxnagazumi 3:8b9ba783512e 129 }
maxnagazumi 3:8b9ba783512e 130 private:
maxnagazumi 20:acb13e232678 131 double max_,vx_,vy_,theta_,k_;
maxnagazumi 3:8b9ba783512e 132 double omega[4];
maxnagazumi 3:8b9ba783512e 133 };
maxnagazumi 3:8b9ba783512e 134
maxnagazumi 3:8b9ba783512e 135 WheelOmega omega;
maxnagazumi 18:1d89ec4148ec 136 //パラメタ処理
maxnagazumi 20:acb13e232678 137 double pControl(double distance_,double zMin,double newtime)
maxnagazumi 16:7ce4ab00621a 138 {
maxnagazumi 20:acb13e232678 139 double z,zMax,olddistance,oldtime;
maxnagazumi 20:acb13e232678 140 double diftime_;
maxnagazumi 20:acb13e232678 141 diftime_ = newtime - oldtime;
maxnagazumi 20:acb13e232678 142 oldtime= newtime;
maxnagazumi 20:acb13e232678 143 z=0.004*distance_ - 0.004*(olddistance-distance_)/diftime_;
maxnagazumi 22:5c78d75fb2f2 144 zMax=10;
maxnagazumi 16:7ce4ab00621a 145 if(z>zMax) {
maxnagazumi 16:7ce4ab00621a 146 z=zMax;
maxnagazumi 16:7ce4ab00621a 147 }
maxnagazumi 20:acb13e232678 148 if(z<zMin) {
maxnagazumi 20:acb13e232678 149 z=zMin;
maxnagazumi 16:7ce4ab00621a 150 }
maxnagazumi 20:acb13e232678 151 if(newtime<1) {
maxnagazumi 20:acb13e232678 152 z=z*newtime;
maxnagazumi 20:acb13e232678 153 }
maxnagazumi 20:acb13e232678 154 olddistance = distance_;
maxnagazumi 16:7ce4ab00621a 155 return z;
maxnagazumi 16:7ce4ab00621a 156 }
koheim 11:f9f3014598b9 157
maxnagazumi 18:1d89ec4148ec 158 //出力
maxnagazumi 18:1d89ec4148ec 159 int a=0;
maxnagazumi 18:1d89ec4148ec 160 void motorOut()
maxnagazumi 18:1d89ec4148ec 161 {
maxnagazumi 18:1d89ec4148ec 162 for(int i=0; i<4; i++) {
maxnagazumi 18:1d89ec4148ec 163 motor[i].Sc(omega.getOmega(i));
maxnagazumi 18:1d89ec4148ec 164 }
maxnagazumi 18:1d89ec4148ec 165 }
maxnagazumi 18:1d89ec4148ec 166
maxnagazumi 0:c17bc30288a2 167 int main()
maxnagazumi 0:c17bc30288a2 168 {
maxnagazumi 1:9d2b2b5ec36f 169 gyro.initialize(); //main関数の最初に一度だけ実行
maxnagazumi 1:9d2b2b5ec36f 170 gyro.acc_offset();
maxnagazumi 20:acb13e232678 171 double angle;
maxnagazumi 20:acb13e232678 172 angle=gyro.getAngle();
maxnagazumi 18:1d89ec4148ec 173 double z;
maxnagazumi 0:c17bc30288a2 174 printf("start\r\n");
maxnagazumi 0:c17bc30288a2 175 motor[0].setEquation(0.008031,-0.022300,-0.008839,-0.016290);
maxnagazumi 0:c17bc30288a2 176 motor[1].setEquation(0.008878,-0.016622,-0.009702,-0.015806);
maxnagazumi 0:c17bc30288a2 177 motor[2].setEquation(0.008637,-0.016537,-0.009397,-0.012159);
maxnagazumi 0:c17bc30288a2 178 motor[3].setEquation(0.008096,-0.014822,-0.008801,-0.016645);
maxnagazumi 0:c17bc30288a2 179
maxnagazumi 0:c17bc30288a2 180 motor[0].setDutyLimit(0.5);
maxnagazumi 0:c17bc30288a2 181 motor[1].setDutyLimit(0.5);
maxnagazumi 0:c17bc30288a2 182 motor[2].setDutyLimit(0.5);
maxnagazumi 0:c17bc30288a2 183 motor[3].setDutyLimit(0.5);
maxnagazumi 0:c17bc30288a2 184
maxnagazumi 20:acb13e232678 185 motor[0].setPDparam( 0.1790, 0.00560);
maxnagazumi 20:acb13e232678 186 motor[1].setPDparam( 0.1705, 0.00620);
maxnagazumi 20:acb13e232678 187 motor[2].setPDparam( 0.1790, 0.00620);
maxnagazumi 20:acb13e232678 188 motor[3].setPDparam( 0.1680, 0.00560);
maxnagazumi 0:c17bc30288a2 189
maxnagazumi 3:8b9ba783512e 190 while(1) {
maxnagazumi 3:8b9ba783512e 191 printf("waiting\r\n");
maxnagazumi 3:8b9ba783512e 192 if(button==0) {
maxnagazumi 4:6a8cd13da02d 193 wait(1);
maxnagazumi 3:8b9ba783512e 194 ticker.attach(motorOut,0.05);
maxnagazumi 3:8b9ba783512e 195 break;
maxnagazumi 3:8b9ba783512e 196 }
maxnagazumi 3:8b9ba783512e 197 }
maxnagazumi 3:8b9ba783512e 198
maxnagazumi 3:8b9ba783512e 199 int n=1,dx,dy,aimX,aimY;
maxnagazumi 22:5c78d75fb2f2 200 double vx_,vy_,vx,vy,newtime,distance;
maxnagazumi 0:c17bc30288a2 201 Location location;
maxnagazumi 16:7ce4ab00621a 202 Timer time;
maxnagazumi 16:7ce4ab00621a 203 time.start();
maxnagazumi 0:c17bc30288a2 204 while(1) {
maxnagazumi 0:c17bc30288a2 205 //自己位置取得
maxnagazumi 18:1d89ec4148ec 206 theta=gyro.getAngle()-angle; //角度の値を受け取る
maxnagazumi 0:c17bc30288a2 207 location.calXY();
maxnagazumi 1:9d2b2b5ec36f 208
maxnagazumi 0:c17bc30288a2 209 x=location.getX();
maxnagazumi 0:c17bc30288a2 210 y=location.getY();
maxnagazumi 22:5c78d75fb2f2 211 printf("X=%d,Y=%d,theta=%5.3f z=%5.3f %f\r\n",x,y,theta,z,time.read());
maxnagazumi 1:9d2b2b5ec36f 212
maxnagazumi 0:c17bc30288a2 213 //目的地決定(syuusoku check)
maxnagazumi 0:c17bc30288a2 214 aimX = plot[n][0];
maxnagazumi 0:c17bc30288a2 215 aimY = plot[n][1];
maxnagazumi 0:c17bc30288a2 216 //出力を計算(kitai xy);
maxnagazumi 0:c17bc30288a2 217 dx=aimX-x;
maxnagazumi 0:c17bc30288a2 218 dy=aimY-y;
maxnagazumi 22:5c78d75fb2f2 219 vx_=dx/sqrt((double)dx*dx+dy*dy);
maxnagazumi 22:5c78d75fb2f2 220 vy_=dy/sqrt((double)dx*dx+dy*dy);
maxnagazumi 22:5c78d75fb2f2 221 vx=vx_*cos(theta/360*3.14)+vy_*sin(theta/180*3.14);
maxnagazumi 22:5c78d75fb2f2 222 vy=vx_*sin(theta/360*3.14)+vy_*cos(theta/180*3.14);
maxnagazumi 1:9d2b2b5ec36f 223 //四輪の出力計算
maxnagazumi 20:acb13e232678 224 newtime=time.read();
maxnagazumi 20:acb13e232678 225 distance = sqrt((float)dx*dx+dy*dy);
maxnagazumi 20:acb13e232678 226 z=pControl(distance,zMin[n],newtime);
maxnagazumi 22:5c78d75fb2f2 227 omega.setOmega(z,0.05);
maxnagazumi 20:acb13e232678 228 omega.setVxy(vx,vy,aimTheta[n]);
maxnagazumi 3:8b9ba783512e 229 omega.calOmega();
maxnagazumi 18:1d89ec4148ec 230 //ゴール判定
maxnagazumi 20:acb13e232678 231 if(distance<400) {
maxnagazumi 16:7ce4ab00621a 232 n++;
maxnagazumi 18:1d89ec4148ec 233 printf("reach%d\r\n",n);
maxnagazumi 22:5c78d75fb2f2 234 time.reset();
koheim 15:e26bad257626 235 }
maxnagazumi 16:7ce4ab00621a 236
maxnagazumi 18:1d89ec4148ec 237 if(n>=5) {
maxnagazumi 1:9d2b2b5ec36f 238 for(int j=0; j<4; j++) {
maxnagazumi 4:6a8cd13da02d 239 motor[j].Sc(0);
maxnagazumi 4:6a8cd13da02d 240 }
maxnagazumi 4:6a8cd13da02d 241 while(1) {
maxnagazumi 20:acb13e232678 242
maxnagazumi 4:6a8cd13da02d 243 printf("fin");
maxnagazumi 9:04291120b9ad 244 ticker.detach();
maxnagazumi 1:9d2b2b5ec36f 245 }
maxnagazumi 1:9d2b2b5ec36f 246 }
maxnagazumi 0:c17bc30288a2 247 }
maxnagazumi 0:c17bc30288a2 248 }