春ロボ　ロケット団 / 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@19:0c1c3b2e009f, 2020-03-05 (annotated)

Committer:
koheim
Date:
Thu Mar 05 13:09:29 2020 +0000
Revision:
19:0c1c3b2e009f
Parent:
18:1d89ec4148ec
Child:
20:acb13e232678
a;

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