ROBOSTEP_SHARE
/
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
Revision 23:43672349ceed, committed 2020-11-21
- Comitter:
- maxnagazumi
- Date:
- Sat Nov 21 08:54:05 2020 +0000
- Parent:
- 22:5c78d75fb2f2
- Commit message:
- motor test
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Mar 09 11:56:16 2020 +0000
+++ b/main.cpp Sat Nov 21 08:54:05 2020 +0000
@@ -5,21 +5,24 @@
#include "math.h"
#include "R1370P.h"
-Ec2multi ec[]= {Ec2multi(PC_5,PB_2,RESOLUTION),
- Ec2multi(PA_11,PB_1,RESOLUTION),
- Ec2multi(PB_12,PB_15,RESOLUTION),
- Ec2multi(PC_4,PB_14,RESOLUTION)
- }; //2逓倍用class
+Ec2multi ec[]= {
+ Ec2multi(PC_5,PB_2,RESOLUTION),
+ Ec2multi(PA_11,PB_1,RESOLUTION),
+ Ec2multi(PB_12,PB_15,RESOLUTION),
+ Ec2multi(PC_4,PB_14,RESOLUTION)
+}; //2逓倍用class
-Ec2multi ecXY[]= {Ec2multi(PC_6,PB_8,RESOLUTION),
- Ec2multi(PC_8,PB_9,RESOLUTION)
- };
+Ec2multi ecXY[]= {
+ Ec2multi(PC_6,PB_8,RESOLUTION),
+ Ec2multi(PC_8,PB_9,RESOLUTION)
+};
-SpeedControl motor[]= {SpeedControl(PA_5,PC_7,50,ec[0]),
- SpeedControl(PC_9,PA_1,50,ec[1]),
- SpeedControl(PA_10,PB_4,50,ec[2]),
- SpeedControl(PA_9,PA_7,50,ec[3])
- };
+SpeedControl motor[]= {
+ SpeedControl(PA_5,PC_7,50,ec[0]),
+ SpeedControl(PC_9,PA_1,50,ec[1]),
+ SpeedControl(PA_10,PB_4,50,ec[2]),
+ SpeedControl(PA_9,PA_7,50,ec[3])
+};
DigitalIn button(USER_BUTTON);
Serial pc(USBTX, USBRX); // tx, rx
@@ -75,20 +78,20 @@
//目的地決定
-int plot[5][2]= {
+int plot[][2]= {
{0,0}
- ,{0,5000}
- ,{5000,5000}
- ,{5000,0}
- ,{2000,5000}
+ ,{500,10500}
+ ,{800,13500}
+ ,{1000,15500}
+ ,{8654,16500}
};
-double aimTheta[5]= {//目標角度を指定
- 0,-30,-180,-90,0
+double aimTheta[]= {//目標角度を指定
+ 0,0,0, 0,0,0, 0,0,0, 0,0,0,
};
-double zMin[5]= { //速度の最少を指定
- 0,2,2,2,0
+double zMin[]= { //速度の最少を指定
+ 2,2,2, 2,2,2, 5,5,5, 5,5,5
};
//出力を計算
@@ -114,6 +117,12 @@
vx_=vx;
vy_=vy;
theta_=aimtheta_ - theta;
+ if(theta_>30) {//目標角度まで３０度以上空いていたら補正、係数調整のため３０は適当
+ theta_=30;
+ }
+ if(theta_<-30) {
+ theta_=-30;
+ }
}
void calOmega()
{
@@ -140,8 +149,8 @@
double diftime_;
diftime_ = newtime - oldtime;
oldtime= newtime;
- z=0.004*distance_ - 0.004*(olddistance-distance_)/diftime_;
- zMax=10;
+ z=0.004*distance_ - 0.1*(olddistance-distance_)/diftime_;
+ zMax=25;
if(z>zMax) {
z=zMax;
}
@@ -157,11 +166,18 @@
//出力
int a=0;
+int j=0;
void motorOut()
{
+ printf("\r\n");
for(int i=0; i<4; i++) {
motor[i].Sc(omega.getOmega(i));
+ if(j%10==1) {
+ printf(" %d",ec[i].getCount());
+ }
}
+ j++;
+ printf("\r\n");
}
int main()
@@ -177,10 +193,10 @@
motor[2].setEquation(0.008637,-0.016537,-0.009397,-0.012159);
motor[3].setEquation(0.008096,-0.014822,-0.008801,-0.016645);
- motor[0].setDutyLimit(0.5);
- motor[1].setDutyLimit(0.5);
- motor[2].setDutyLimit(0.5);
- motor[3].setDutyLimit(0.5);
+ motor[0].setDutyLimit(0.4);
+ motor[1].setDutyLimit(0.4);
+ motor[2].setDutyLimit(0.4);
+ motor[3].setDutyLimit(0.4);
motor[0].setPDparam( 0.1790, 0.00560);
motor[1].setPDparam( 0.1705, 0.00620);
@@ -218,8 +234,8 @@
dy=aimY-y;
vx_=dx/sqrt((double)dx*dx+dy*dy);
vy_=dy/sqrt((double)dx*dx+dy*dy);
- vx=vx_*cos(theta/360*3.14)+vy_*sin(theta/180*3.14);
- vy=vx_*sin(theta/360*3.14)+vy_*cos(theta/180*3.14);
+ vx=vx_*cos(theta/180*3.14)+vy_*sin(theta/180*3.14);
+ vy=-vx_*sin(theta/180*3.14)+vy_*cos(theta/180*3.14);
//四輪の出力計算
newtime=time.read();
distance = sqrt((float)dx*dx+dy*dy);
@@ -228,7 +244,7 @@
omega.setVxy(vx,vy,aimTheta[n]);
omega.calOmega();
//ゴール判定
- if(distance<400) {
+ if(distance<800) {
n++;
printf("reach%d\r\n",n);
time.reset();
@@ -238,11 +254,9 @@
for(int j=0; j<4; j++) {
motor[j].Sc(0);
}
- while(1) {
-
- printf("fin");
- ticker.detach();
- }
+ printf("fin\r\n");
+ ticker.detach();
}
}
}
+