春ロボ　ロケット団 / 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@2:08b8f6b05c22, 2020-03-04 (annotated)

Committer:
koheim
Date:
Wed Mar 04 06:06:07 2020 +0000
Revision:
2:08b8f6b05c22
Parent:
1:9d2b2b5ec36f 
sonic

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"
koheim 2:08b8f6b05c22 7 #include"hcsr04.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 0:c17bc30288a2 25 Ec2multi(PB_9,PC_8,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
koheim 2:08b8f6b05c22 34 HCSR04 echo[]= {HCSR04(PC_0,PC_12),//A
koheim 2:08b8f6b05c22 35 HCSR04(PA_15,PB_7),//A
koheim 2:08b8f6b05c22 36 HCSR04(PH_1,PB_0),// B
koheim 2:08b8f6b05c22 37 HCSR04(PC_3,PB_10),//B
koheim 2:08b8f6b05c22 38 };
maxnagazumi 0:c17bc30288a2 39
maxnagazumi 0:c17bc30288a2 40 DigitalIn button(USER_BUTTON);
maxnagazumi 1:9d2b2b5ec36f 41 Serial pc(USBTX, USBRX); // tx, rx
maxnagazumi 1:9d2b2b5ec36f 42 R1370P gyro(PC_10,PC_11); // tx, rx
maxnagazumi 0:c17bc30288a2 43
maxnagazumi 0:c17bc30288a2 44 Ticker ticker;
maxnagazumi 0:c17bc30288a2 45
maxnagazumi 0:c17bc30288a2 46 void calOmega()
maxnagazumi 0:c17bc30288a2 47 {
maxnagazumi 0:c17bc30288a2 48 for(int i=0; i<4; i++) {
maxnagazumi 0:c17bc30288a2 49 ec[i].calOmega();
maxnagazumi 0:c17bc30288a2 50 }
maxnagazumi 0:c17bc30288a2 51 }
maxnagazumi 0:c17bc30288a2 52
maxnagazumi 0:c17bc30288a2 53 //自己位置取得
maxnagazumi 1:9d2b2b5ec36f 54 double theta=0;
maxnagazumi 0:c17bc30288a2 55 class Location
maxnagazumi 0:c17bc30288a2 56 {
maxnagazumi 0:c17bc30288a2 57 public:
maxnagazumi 0:c17bc30288a2 58 Location():x_(0),y_(0)
maxnagazumi 0:c17bc30288a2 59 {
maxnagazumi 0:c17bc30288a2 60 for(int i =0; i<2; i++) {
maxnagazumi 0:c17bc30288a2 61 old_count[i]=0;
maxnagazumi 0:c17bc30288a2 62 }
maxnagazumi 0:c17bc30288a2 63 }
maxnagazumi 0:c17bc30288a2 64 void calXY()
maxnagazumi 0:c17bc30288a2 65 {
maxnagazumi 0:c17bc30288a2 66 double ec_count[2]= {};
maxnagazumi 1:9d2b2b5ec36f 67 double ax,ay,bx,by;
maxnagazumi 0:c17bc30288a2 68 ec_count[0]=ecXY[0].getCount();
maxnagazumi 0:c17bc30288a2 69 ec_count[1]=ecXY[1].getCount();
maxnagazumi 1:9d2b2b5ec36f 70 ax = (ec_count[0]-old_count[0])*cos(45+theta);
maxnagazumi 1:9d2b2b5ec36f 71 ay = (ec_count[0]-old_count[0])*sin(45+theta);
maxnagazumi 1:9d2b2b5ec36f 72 bx = (ec_count[1]-old_count[1])*cos(45-theta);
maxnagazumi 1:9d2b2b5ec36f 73 by = (ec_count[1]-old_count[1])*sin(45-theta);
maxnagazumi 1:9d2b2b5ec36f 74 x_=x_+ax + bx;
maxnagazumi 1:9d2b2b5ec36f 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 1:9d2b2b5ec36f 99 ,{0,10000}
maxnagazumi 1:9d2b2b5ec36f 100 ,{5000,10000}
maxnagazumi 1:9d2b2b5ec36f 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 double omega[4];
maxnagazumi 0:c17bc30288a2 107 int x,y;
maxnagazumi 1:9d2b2b5ec36f 108 //出力
maxnagazumi 1:9d2b2b5ec36f 109 double vx,vy;
maxnagazumi 1:9d2b2b5ec36f 110 void setOmega(int max)
maxnagazumi 1:9d2b2b5ec36f 111 {
maxnagazumi 1:9d2b2b5ec36f 112 omega[0]=max*vx/sqrt(2.0)-max*vy/sqrt(2.0);
maxnagazumi 1:9d2b2b5ec36f 113 omega[1]=-max*vx/sqrt(2.0)-max*vy/sqrt(2.0);
maxnagazumi 1:9d2b2b5ec36f 114 omega[2]=-max*vx/sqrt(2.0)+max*vy/sqrt(2.0);
maxnagazumi 1:9d2b2b5ec36f 115 omega[3]=max*vx/sqrt(2.0)+max*vy/sqrt(2.0);
maxnagazumi 1:9d2b2b5ec36f 116 }
maxnagazumi 0:c17bc30288a2 117 void motorOut()
maxnagazumi 0:c17bc30288a2 118 {
maxnagazumi 0:c17bc30288a2 119 for(int i=0; i<4; i++) {
maxnagazumi 1:9d2b2b5ec36f 120 motor[i].Sc((int)omega[i]);
maxnagazumi 0:c17bc30288a2 121 }
maxnagazumi 0:c17bc30288a2 122 }
koheim 2:08b8f6b05c22 123 void echo_()
koheim 2:08b8f6b05c22 124 {
koheim 2:08b8f6b05c22 125 double a=0,b=0,c=0,d=0;
koheim 2:08b8f6b05c22 126 for(int i=0; i<4; i++) {
koheim 2:08b8f6b05c22 127 echo[i].start();
koheim 2:08b8f6b05c22 128 }
koheim 2:08b8f6b05c22 129 wait(1);//minimum wait is 30msec
koheim 2:08b8f6b05c22 130 a=echo[0].get_dist_cm();
koheim 2:08b8f6b05c22 131 b=echo[1].get_dist_cm();
koheim 2:08b8f6b05c22 132 c=echo[2].get_dist_cm();
koheim 2:08b8f6b05c22 133 d=echo[3].get_dist_cm();
koheim 2:08b8f6b05c22 134 printf("a:%f b:%f c:%f d:%f\r\n",a,b,c,d);
koheim 2:08b8f6b05c22 135 if(a<5||b<5||c<5||d<5) {
koheim 2:08b8f6b05c22 136 for(int j=0; j<4; j++) {
koheim 2:08b8f6b05c22 137 motor[j].stop();
koheim 2:08b8f6b05c22 138 }
koheim 2:08b8f6b05c22 139 }
koheim 2:08b8f6b05c22 140 }
koheim 2:08b8f6b05c22 141
maxnagazumi 0:c17bc30288a2 142 int main()
maxnagazumi 0:c17bc30288a2 143 {
maxnagazumi 1:9d2b2b5ec36f 144 gyro.initialize(); //main関数の最初に一度だけ実行
maxnagazumi 1:9d2b2b5ec36f 145 gyro.acc_offset();
maxnagazumi 1:9d2b2b5ec36f 146 ticker.attach(motorOut,0.05);
maxnagazumi 0:c17bc30288a2 147 printf("start\r\n");
maxnagazumi 0:c17bc30288a2 148 motor[0].setEquation(0.008031,-0.022300,-0.008839,-0.016290);
maxnagazumi 0:c17bc30288a2 149 motor[1].setEquation(0.008878,-0.016622,-0.009702,-0.015806);
maxnagazumi 0:c17bc30288a2 150 motor[2].setEquation(0.008637,-0.016537,-0.009397,-0.012159);
maxnagazumi 0:c17bc30288a2 151 motor[3].setEquation(0.008096,-0.014822,-0.008801,-0.016645);
maxnagazumi 0:c17bc30288a2 152
maxnagazumi 0:c17bc30288a2 153 motor[0].setDutyLimit(0.5);
maxnagazumi 0:c17bc30288a2 154 motor[1].setDutyLimit(0.5);
maxnagazumi 0:c17bc30288a2 155 motor[2].setDutyLimit(0.5);
maxnagazumi 0:c17bc30288a2 156 motor[3].setDutyLimit(0.5);
maxnagazumi 0:c17bc30288a2 157
maxnagazumi 0:c17bc30288a2 158 motor[0].setPDparam( 0.004839, 0.0026290 );
maxnagazumi 0:c17bc30288a2 159 motor[1].setPDparam( 0.004702, 0.025806 );
maxnagazumi 0:c17bc30288a2 160 motor[2].setPDparam( 0.004397, 0.025159 );
maxnagazumi 0:c17bc30288a2 161 motor[3].setPDparam( 0.004801, 0.026645 );
maxnagazumi 0:c17bc30288a2 162
maxnagazumi 0:c17bc30288a2 163 int n=0,dx,dy,aimX,aimY;
koheim 2:08b8f6b05c22 164 double a=0,b=0,c=0,d=0;
maxnagazumi 0:c17bc30288a2 165 Location location;
maxnagazumi 0:c17bc30288a2 166 while(1) {
maxnagazumi 0:c17bc30288a2 167 //自己位置取得
maxnagazumi 1:9d2b2b5ec36f 168 theta=gyro.getAngle(); //角度の値を受け取る
maxnagazumi 0:c17bc30288a2 169 location.calXY();
maxnagazumi 1:9d2b2b5ec36f 170
maxnagazumi 0:c17bc30288a2 171 x=location.getX();
maxnagazumi 0:c17bc30288a2 172 y=location.getY();
maxnagazumi 1:9d2b2b5ec36f 173 printf("%3.3f %3.3f %3.3f %3.3f location %d,%d \r\n",omega[0],omega[1],omega[2],omega[3],x,y);
maxnagazumi 1:9d2b2b5ec36f 174
maxnagazumi 0:c17bc30288a2 175 //目的地決定(syuusoku check)
maxnagazumi 0:c17bc30288a2 176 aimX = plot[n][0];
maxnagazumi 0:c17bc30288a2 177 aimY = plot[n][1];
maxnagazumi 0:c17bc30288a2 178 //出力を計算(kitai xy);
maxnagazumi 0:c17bc30288a2 179 dx=aimX-x;
maxnagazumi 0:c17bc30288a2 180 dy=aimY-y;
maxnagazumi 0:c17bc30288a2 181 vx=dx/sqrt((double)dx*dx+dy*dy);
maxnagazumi 0:c17bc30288a2 182 vy=dy/sqrt((double)dx*dx+dy*dy);
maxnagazumi 0:c17bc30288a2 183
maxnagazumi 1:9d2b2b5ec36f 184 //四輪の出力計算
maxnagazumi 1:9d2b2b5ec36f 185 setOmega(10);
maxnagazumi 1:9d2b2b5ec36f 186
maxnagazumi 1:9d2b2b5ec36f 187 if(dx<100 &&dx>-100) {
maxnagazumi 1:9d2b2b5ec36f 188 if(dy<100 && dy>-100) {
maxnagazumi 0:c17bc30288a2 189 n++;
maxnagazumi 0:c17bc30288a2 190 printf("reached Location");
maxnagazumi 1:9d2b2b5ec36f 191 for(int j=0; j<4; j++) {
maxnagazumi 0:c17bc30288a2 192 motor[j].stop();
maxnagazumi 0:c17bc30288a2 193 }
maxnagazumi 1:9d2b2b5ec36f 194 wait(2);
maxnagazumi 0:c17bc30288a2 195 }
maxnagazumi 0:c17bc30288a2 196
maxnagazumi 0:c17bc30288a2 197 }
maxnagazumi 1:9d2b2b5ec36f 198 if(n>4) {
maxnagazumi 1:9d2b2b5ec36f 199 for(int j=0; j<4; j++) {
maxnagazumi 1:9d2b2b5ec36f 200 motor[j].stop();
maxnagazumi 1:9d2b2b5ec36f 201 }
maxnagazumi 1:9d2b2b5ec36f 202 }
maxnagazumi 0:c17bc30288a2 203 }
maxnagazumi 0:c17bc30288a2 204
maxnagazumi 0:c17bc30288a2 205 }