ROBOSTEP_SHARE / 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@23:43672349ceed, 2020-11-21 (annotated)

Committer:
maxnagazumi
Date:
Sat Nov 21 08:54:05 2020 +0000
Revision:
23:43672349ceed
Parent:
22:5c78d75fb2f2 
motor test

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