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

Committer:
maxnagazumi
Date:
Wed Mar 04 02:07:55 2020 +0000
Revision:
0:c17bc30288a2
Child:
1:9d2b2b5ec36f
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 0:c17bc30288a2 24 Ec2multi(PB_9,PC_8,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 0:c17bc30288a2 36
maxnagazumi 0:c17bc30288a2 37 Ticker ticker;
maxnagazumi 0:c17bc30288a2 38
maxnagazumi 0:c17bc30288a2 39 void calOmega()
maxnagazumi 0:c17bc30288a2 40 {
maxnagazumi 0:c17bc30288a2 41 for(int i=0; i<4; i++) {
maxnagazumi 0:c17bc30288a2 42 ec[i].calOmega();
maxnagazumi 0:c17bc30288a2 43 }
maxnagazumi 0:c17bc30288a2 44 }
maxnagazumi 0:c17bc30288a2 45
maxnagazumi 0:c17bc30288a2 46 //自己位置取得
maxnagazumi 0:c17bc30288a2 47 class Location
maxnagazumi 0:c17bc30288a2 48 {
maxnagazumi 0:c17bc30288a2 49 public:
maxnagazumi 0:c17bc30288a2 50 Location():x_(0),y_(0)
maxnagazumi 0:c17bc30288a2 51 {
maxnagazumi 0:c17bc30288a2 52 for(int i =0; i<2; i++) {
maxnagazumi 0:c17bc30288a2 53 old_count[i]=0;
maxnagazumi 0:c17bc30288a2 54 }
maxnagazumi 0:c17bc30288a2 55 }
maxnagazumi 0:c17bc30288a2 56 void calXY()
maxnagazumi 0:c17bc30288a2 57 {
maxnagazumi 0:c17bc30288a2 58 double ec_count[2]= {};
maxnagazumi 0:c17bc30288a2 59 double a,b;
maxnagazumi 0:c17bc30288a2 60 ec_count[0]=ecXY[0].getCount();
maxnagazumi 0:c17bc30288a2 61 ec_count[1]=ecXY[1].getCount();
maxnagazumi 0:c17bc30288a2 62 a = (ec_count[0]-old_count[0])/sqrt(2.0);
maxnagazumi 0:c17bc30288a2 63 b = (ec_count[1]-old_count[1])/sqrt(2.0);
maxnagazumi 0:c17bc30288a2 64 x_=x_+a + b;
maxnagazumi 0:c17bc30288a2 65 y_=y_+a - b;
maxnagazumi 0:c17bc30288a2 66 old_count[0]=ec_count[0];
maxnagazumi 0:c17bc30288a2 67 old_count[1]=ec_count[1];
maxnagazumi 0:c17bc30288a2 68 }
maxnagazumi 0:c17bc30288a2 69 double getX()
maxnagazumi 0:c17bc30288a2 70 {
maxnagazumi 0:c17bc30288a2 71 return x_;
maxnagazumi 0:c17bc30288a2 72 }
maxnagazumi 0:c17bc30288a2 73 double getY()
maxnagazumi 0:c17bc30288a2 74 {
maxnagazumi 0:c17bc30288a2 75 return y_;
maxnagazumi 0:c17bc30288a2 76 }
maxnagazumi 0:c17bc30288a2 77
maxnagazumi 0:c17bc30288a2 78 private:
maxnagazumi 0:c17bc30288a2 79 double x_;
maxnagazumi 0:c17bc30288a2 80 double y_;
maxnagazumi 0:c17bc30288a2 81 double old_count[2];
maxnagazumi 0:c17bc30288a2 82 };
maxnagazumi 0:c17bc30288a2 83
maxnagazumi 0:c17bc30288a2 84
maxnagazumi 0:c17bc30288a2 85 int plot[5][2]= {
maxnagazumi 0:c17bc30288a2 86 {0,0}
maxnagazumi 0:c17bc30288a2 87 ,{0,3000}
maxnagazumi 0:c17bc30288a2 88 ,{3000,3000}
maxnagazumi 0:c17bc30288a2 89 ,{3000,0}
maxnagazumi 0:c17bc30288a2 90 ,{0,0}
maxnagazumi 0:c17bc30288a2 91 };
maxnagazumi 0:c17bc30288a2 92
maxnagazumi 0:c17bc30288a2 93 //目的地決定
maxnagazumi 0:c17bc30288a2 94 /*class Aim
maxnagazumi 0:c17bc30288a2 95 {
maxnagazumi 0:c17bc30288a2 96 Aim(int plot[][2]):plot_(&plot[0][0]) {}
maxnagazumi 0:c17bc30288a2 97 public:
maxnagazumi 0:c17bc30288a2 98 private:
maxnagazumi 0:c17bc30288a2 99 int *(plot_[2]);
maxnagazumi 0:c17bc30288a2 100 };*/
maxnagazumi 0:c17bc30288a2 101
maxnagazumi 0:c17bc30288a2 102 //出力を計算
maxnagazumi 0:c17bc30288a2 103 double omega[4];
maxnagazumi 0:c17bc30288a2 104 int x,y;
maxnagazumi 0:c17bc30288a2 105 void motorOut()
maxnagazumi 0:c17bc30288a2 106 {
maxnagazumi 0:c17bc30288a2 107 for(int i=0; i<4; i++) {
maxnagazumi 0:c17bc30288a2 108 motor[i].Sc(omega[i]);
maxnagazumi 0:c17bc30288a2 109 }
maxnagazumi 0:c17bc30288a2 110 }
maxnagazumi 0:c17bc30288a2 111 int main()
maxnagazumi 0:c17bc30288a2 112 {
maxnagazumi 0:c17bc30288a2 113 ticker.attach(motorOut,0.09);
maxnagazumi 0:c17bc30288a2 114 printf("start\r\n");
maxnagazumi 0:c17bc30288a2 115 motor[0].setEquation(0.008031,-0.022300,-0.008839,-0.016290);
maxnagazumi 0:c17bc30288a2 116 motor[1].setEquation(0.008878,-0.016622,-0.009702,-0.015806);
maxnagazumi 0:c17bc30288a2 117 motor[2].setEquation(0.008637,-0.016537,-0.009397,-0.012159);
maxnagazumi 0:c17bc30288a2 118 motor[3].setEquation(0.008096,-0.014822,-0.008801,-0.016645);
maxnagazumi 0:c17bc30288a2 119
maxnagazumi 0:c17bc30288a2 120 motor[0].setDutyLimit(0.5);
maxnagazumi 0:c17bc30288a2 121 motor[1].setDutyLimit(0.5);
maxnagazumi 0:c17bc30288a2 122 motor[2].setDutyLimit(0.5);
maxnagazumi 0:c17bc30288a2 123 motor[3].setDutyLimit(0.5);
maxnagazumi 0:c17bc30288a2 124
maxnagazumi 0:c17bc30288a2 125 motor[0].setPDparam( 0.004839, 0.0026290 );
maxnagazumi 0:c17bc30288a2 126 motor[1].setPDparam( 0.004702, 0.025806 );
maxnagazumi 0:c17bc30288a2 127 motor[2].setPDparam( 0.004397, 0.025159 );
maxnagazumi 0:c17bc30288a2 128 motor[3].setPDparam( 0.004801, 0.026645 );
maxnagazumi 0:c17bc30288a2 129
maxnagazumi 0:c17bc30288a2 130 int n=0,dx,dy,aimX,aimY;
maxnagazumi 0:c17bc30288a2 131 double vx,vy;
maxnagazumi 0:c17bc30288a2 132 Location location;
maxnagazumi 0:c17bc30288a2 133 // Aim aim(&(plot[0]));
maxnagazumi 0:c17bc30288a2 134 while(1) {
maxnagazumi 0:c17bc30288a2 135 //自己位置取得
maxnagazumi 0:c17bc30288a2 136 location.calXY();
maxnagazumi 0:c17bc30288a2 137 x=location.getX();
maxnagazumi 0:c17bc30288a2 138 y=location.getY();
maxnagazumi 0:c17bc30288a2 139 printf("%lf %lf %lf %lf location %d,%d \r\n",omega[0],omega[1],omega[2],omega[3],x,y);
maxnagazumi 0:c17bc30288a2 140 //目的地決定(syuusoku check)
maxnagazumi 0:c17bc30288a2 141 aimX = plot[n][0];
maxnagazumi 0:c17bc30288a2 142 aimY = plot[n][1];
maxnagazumi 0:c17bc30288a2 143 //出力を計算(kitai xy);
maxnagazumi 0:c17bc30288a2 144 dx=aimX-x;
maxnagazumi 0:c17bc30288a2 145 dy=aimY-y;
maxnagazumi 0:c17bc30288a2 146 vx=dx/sqrt((double)dx*dx+dy*dy);
maxnagazumi 0:c17bc30288a2 147 vy=dy/sqrt((double)dx*dx+dy*dy);
maxnagazumi 0:c17bc30288a2 148
maxnagazumi 0:c17bc30288a2 149 //cal 4 wheel
maxnagazumi 0:c17bc30288a2 150 omega[0]= -30*vx/sqrt(2.0)+30*vy/sqrt(2.0);
maxnagazumi 0:c17bc30288a2 151 omega[1]=-30*vx/sqrt(2.0)-30*vy/sqrt(2.0);
maxnagazumi 0:c17bc30288a2 152 omega[2]= 30*vx/sqrt(2.0)-30*vy/sqrt(2.0);
maxnagazumi 0:c17bc30288a2 153 omega[3]= 30*vx/sqrt(2.0)+30*vy/sqrt(2.0);
maxnagazumi 0:c17bc30288a2 154
maxnagazumi 0:c17bc30288a2 155 int j=0;
maxnagazumi 0:c17bc30288a2 156 if(dx<50 &&dx>-50) {
maxnagazumi 0:c17bc30288a2 157 if(dy<50 && dy>-50) {
maxnagazumi 0:c17bc30288a2 158 n++;
maxnagazumi 0:c17bc30288a2 159 printf("reached Location");
maxnagazumi 0:c17bc30288a2 160 while(j<4) {
maxnagazumi 0:c17bc30288a2 161 motor[j].stop();
maxnagazumi 0:c17bc30288a2 162 j++;
maxnagazumi 0:c17bc30288a2 163 }
maxnagazumi 0:c17bc30288a2 164 wait(1);
maxnagazumi 0:c17bc30288a2 165 }
maxnagazumi 0:c17bc30288a2 166
maxnagazumi 0:c17bc30288a2 167 }
maxnagazumi 0:c17bc30288a2 168 }
maxnagazumi 0:c17bc30288a2 169
maxnagazumi 0:c17bc30288a2 170 }