kouhei umezawa
2022 NHK Bteam main totyuu
Dependencies: HOSOKIkikou FEP_RX22 QEI R1370 ikarashiMDC_2byte_ver mbed 2022_NHK_B_canTR PID SEKIkikou
Revision 8:e1f1a91e9353, committed 2022-10-12
- Comitter:
- umekou
- Date:
- Wed Oct 12 16:05:05 2022 +0000
- Parent:
- 7:76790bcece4b
- Child:
- 9:88f6351221ed
- Commit message:
- 10gatu 12nitini ni koutani setumeisita dousawo suru puroguramudesu
Changed in this revision
| SEKIkikou.lib | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/SEKIkikou.lib Wed Oct 12 00:22:35 2022 +0000 +++ b/SEKIkikou.lib Wed Oct 12 16:05:05 2022 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/SEKIkikou/#3745c060228e +https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/SEKIkikou/#b67cb3335dc7
--- a/main.cpp Wed Oct 12 00:22:35 2022 +0000
+++ b/main.cpp Wed Oct 12 16:05:05 2022 +0000
@@ -16,7 +16,6 @@
QEI enc3(encoder3_A, encoder3_B, NC, 100, QEI::X4_ENCODING);
QEI enc4(encoder4_A, encoder4_B, NC, 100, QEI::X4_ENCODING);
PID front(2.0, 0, 0.001, 0.01);
-//PID rink(2.0, 0, 0.001, 0.01);
DigitalOut stop(stop_pin);
DigitalOut led(LED2);
@@ -46,11 +45,12 @@
for (int i=0; i<8; i++) b[i]=0;
for (int i=0; i<4; i++) stick[i]=0;
}
- canTR();
-// for (int i=0; i<4; i++) pc.printf("%d ", stick[i]);
-// pc.printf(" | ");
-// if (mycon.getStatus()) pc.printf("received\r\n");
-// else pc.printf("anything error...\r\n");
+// canTR();
+ /*
+ for (int i=0; i<4; i++) pc.printf("%d ", stick[i]);
+ pc.printf(" | ");
+ if (mycon.getStatus()) pc.printf("received\r\n");
+ else pc.printf("anything error...\r\n");*/
}
void updateenc(){
@@ -58,7 +58,7 @@
encoderValue[4] = (float)enc2.getPulses();
encoderValue[5] = (float)enc3.getPulses();
encoderValue[6] = (float)enc4.getPulses();
- canTR();
+// canTR();
// for (int i=0; i<4; i++) pc.printf("%d ", encoderValue[i]);
// pc.printf("\r\n ");
}
@@ -66,7 +66,7 @@
void allanglemove(double one,double two,double three,double four){
if(stick[0]>20||stick[0]<-20||stick[1]>20||stick[1]<-20){//全方位移動
for(int i=0; i < 4; i++){
- motorSpeed[i+8] = sin((atan2((double)stick[1],(double)stick[0])-3.14*(i*2-1) / 4)+(frontAngle*3.14/180));
+ motorSpeed[i+8] = sin((atan2((double)stick[0],(double)stick[1])-3.14*(i*2-1) / 4)+(frontAngle*3.14/180));
}
} else {
motorSpeed[8]=one;
@@ -74,7 +74,7 @@
motorSpeed[10]=three;
motorSpeed[11]=four;
}
- canTR();
+// canTR();
}
void PIDset(){
@@ -83,13 +83,6 @@
front.setBias(0);
front.setMode (1);
front.setSetPoint(0);
- /*
- rink.setInputLimits (-180,180);
- rink.setOutputLimits (-1,1);
- rink.setBias(0);
- rink.setMode (1);
- rink.setSetPoint(0);
- */
}
void addPID(){
@@ -97,46 +90,42 @@
for(int i=8; i < 12; i++){
motorSpeed[i]-=front.compute();
}
- canTR();
+// canTR();
}
-/*
-void hosoki(double m8, double m9, double m10, double m11){
- motorSpeed[8]=m8;
- motorSpeed[9]=m9;
- motorSpeed[10]=m10;
- motorSpeed[11]=m11;
-}
-void seki(double m12, double m13, double m14){
- motorSpeed[12]=m12;
- motorSpeed[13]=m13;
- motorSpeed[14]=m14;
- canTR();
-}
-*/
int main()
{
mycon.StartReceive();
+ recieveController();
stop = 1;
led = 1;
- int currentMode=1;
- int bc=0,bn=0;
- int sa=0;
+// int currentMode=1;
+// int bc=0,bn=0;
+// int sa=0;
printf("success!\r\n");
- canAllReset();
+ for(int i=0; i < 8; i++){
+ motor[i].setSpeed(0);
+ }
+
+// canAllReset();
PIDset();
SEKIkikou seki(&motor[6], &motor[4], &motor[5], &b[0], &b[1], &b[2], &b[3], &encoderValue[4], &encoderValue[5], &encoderValue[6]);
- seki.stopAll();
-// HOSOKIkikou hosoki(&motor[10],&motor[11], &motor[8], &motor[9], NULL, NULL, NULL, NULL, NULL, NULL, NULL);
+// seki.stopAll();
+// SEKIkikou seki(&motor[6], &motor[4], &motor[5], NULL, NULL, NULL, NULL, NULL, NULL, NULL);
+
+
+
while(1) {
recieveController();
+ canTR();
updateenc();
+ frontAngle=jyroValue;
-
+ /*
bc=b[7]-bn;
bn=b[7];
@@ -144,6 +133,7 @@
if(bc==1){
currentMode=2;
seki.init(&motor[6],&motor[4], &motor[5], &b[0], &b[1], &b[2], &b[3], &encoderValue[4], &encoderValue[5], &encoderValue[6]);
+ seki.runAll(0,0,0);
allanglemove(0,0,0,0);
}else if(b[0]){
allanglemove(-1,-1,1,1);
@@ -163,6 +153,7 @@
frontAngle-=360;
}
allanglemove(0,0,0,0);
+ seki.runAll(0,0,0);
}else if(b[5]){
sa=0;
frontAngle=jyroValue;
@@ -175,6 +166,7 @@
if(bc==1){
currentMode=3;
seki.stopAll();
+ seki.runAll(0,0,0);
}else{
seki.runAll(-0.3,0.3,0.3);
}
@@ -184,33 +176,58 @@
if(bc==1){
currentMode=1;
seki.stopAll();
+ seki.runAll(0,0,0);
}
+ seki.runAll(0,0,0);*/
allanglemove(0,0,0,0);
addPID();
+// }
+
+ if(b[5]&&b[6]){
+ motorSpeed[15]=0;
+ }else if(b[5]){
+ motorSpeed[15]=0.5;
+ }else if(b[6]){
+ motorSpeed[15]=-0.5;
+ }else{
+ motorSpeed[15]=0;
+ }
+
+
+ if(b[7]==1){
+ seki.runAll(0.1,0.3,-0.3);
+ }else{
+ seki.runAll(-0.8,0.3,0.3);
}
for(int i=0; i < 4; i++){
motor[i].setSpeed(motorSpeed[i+8]*0.8);
}
- for(int i=0; i < 8; i++){
- printf("%d ",b[i]);
+ motor[7].setSpeed(motorSpeed[15]);
+
+ pc.printf("|wheel: ");
+ for(int i=8; i < 12; i++){
+ pc.printf("%.2f ",motorSpeed[i]);
}
- printf("| ");
- for(int i=0; i < 4; i++){
- printf("%d ",stick[i]);
+ pc.printf("|spin: %.2f ",motorSpeed[15]);
+// pc.printf("|updown: %.2f ",motorSpeed[2]);
+ pc.printf("|enc: ");
+ for(int i=0; i < 7; i++){
+ pc.printf("%d ",encoderValue[i]);
+
}
- printf("| ");
- for(int i=2; i < 15; i++){
- printf("%.3f ",motorSpeed[i]);
+ pc.printf("| ");
+ pc.printf("R1370:%.3f",jyroValue);
+ pc.printf("| ");
+ for(int i=0; i < 8; i++){
+ pc.printf("%d ",b[i]);
}
- printf("| ");
- for(int i=0; i < 12; i++){
- printf("%d ",encoderValue[i]);
+ pc.printf("| ");
+ for(int i=0; i < 4; i++){
+ pc.printf("%d ",stick[i]);
}
- printf("| ");
- printf("%.4f\r\n",jyroValue);
+ pc.printf("\r\n");
- canTR();
}
}
\ No newline at end of file