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

Committer:
maxnagazumi
Date:
Thu Mar 05 07:58:01 2020 +0000
Revision:
12:7e11d1d5f22c
Parent:
10:f8e7f39eae0d
Parent:
11:f9f3014598b9
Child:
13:f834d5e83e11
a

Who changed what in which revision?

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