NagaokaRoboticsClub_mbedTeam
Program used by A team in the blue coat of qualifying at NHK in 2019.
Dependencies: SerialMultiByte QEI chatteringremoval omni_wheel prototype01 towelest PID ikarashiMDC_2byte_ver air2 OmniPosition PS3
Diff: main.cpp
- Revision:
- 1:9d1ef7d7f874
- Parent:
- 0:d3db2b0afc76
--- a/main.cpp Mon Sep 23 12:25:26 2019 +0000
+++ b/main.cpp Thu Oct 03 08:42:26 2019 +0000
@@ -1,4 +1,4 @@
-#include "mbed.h" //Red court
+#include "mbed.h" //Blue court
#include "ikarashiMDC.h"
#include "omni_wheel.h"
#include "proto01.h"
@@ -11,23 +11,27 @@
#include "chatteringremoval.h"
#include "pinconfig_main.h"
-PID angle(10.0,0.2,0.000095,0.01);
+Timer time_;
+PID angle(20.0,5.0,0.0000001,0.01);
OmniWheel omni(4);
OmniPosition posi(main_0TX,main_0RX);
Serial serial(mdTX, mdRX, 115200);
RawSerial pc(USBTX, USBRX, 115200);
SerialMultiByte sensor(main_1TX,main_1RX);
-towelest towel(&serial,2,schmitt_trigger_0,schmitt_trigger_1,schmitt_trigger_2);
+towelest towel(&serial,3,schmitt_trigger_0);
air air;
PS3 ps3(FEPTX,FEPRX);
-chatteringremoval button1(1.0);
+chatteringremoval button0(0.3);
+chatteringremoval button1(0.3);
+chatteringremoval button3(0.3);
+chatteringremoval button4(0.3);
DigitalOut sw1(hijoteisi);
DigitalOut LED(LED1);
Timer timer;
int num = 0,X,Y,b[12],stick[4],trigger[2],loli[3];
-int X0,Y0,X1,Y1;
-double valueX, valueY, value[4],mecha_power[4],spin_power=0, IRdistance[2],TFdistance[2],x2, y2;
+int X1,Y1,bb[12],mecha_num = 0,towel_num = 0;
+double X0,Y0,valueX, valueY, value[4],mecha_power[3],spin_power=0,power,theta,IRdistance[2],TFdistance[2],direction = 0.0;
union UnionBytes{
uint8_t bytes[28];
@@ -35,7 +39,7 @@
int32_t TFdist[7]; //2~3
int32_t pulses[7]; //4~6
};
-
+
ikarashiMDC motor[]=
{
ikarashiMDC(0,0,SM,&serial),
@@ -44,11 +48,10 @@
ikarashiMDC(0,3,SM,&serial),
ikarashiMDC(1,0,SM,&serial),
ikarashiMDC(1,1,SM,&serial),
- ikarashiMDC(1,2,SM,&serial),
- ikarashiMDC(1,3,SM,&serial)
+ ikarashiMDC(1,2,SM,&serial)
};
-
-Proto1 proto(500, 700, 0.4, 0.14);
+
+Proto1 proto(500.0, 1000.0, 0.8, 0.14);
void getSensorData() //Receive sensor value
{
@@ -65,140 +68,69 @@
}
}
-void clossmove0() //Adjustment,Airfront
-{
- if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
- x2=0;
- y2=0.3;
- }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
- x2=0;
- y2=-0.3;
- }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
- x2=-0.3;
- y2=0;
- }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
- x2=0.3;
- y2=0;
- }else{
- x2=0;
- y2=0;
- }
-}
-
-void clossmove1() //Adjustment,Recoveryfront
-{
- if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
- x2=0.3;
- y2=0;
- }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
- x2=-0.3;
- y2=0;
- }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
- x2=0;
- y2=0.3;
- }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
- x2=0;
- y2=-0.3;
- }else{
- x2=0;
- y2=0;
- }
-}
-
-void clossmove2() //Adjustment,Dropfront
-{
- if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
- x2=0;
- y2=-0.3;
- }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
- x2=0;
- y2=0.3;
- }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
- x2=0.3;
- y2=0;
- }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
- x2=-0.3;
- y2=0;
- }else{
- x2=0;
- y2=0;
- }
-}
-
-void clossmove3() //Adjustment,Towelfront
-{
- if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
- x2=-0.3;
- y2=0;
- }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
- x2=0.3;
- y2=0;
- }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
- x2=0;
- y2=-0.3;
- }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
- x2=0;
- y2=0.3;
- }else{
- x2=0;
- y2=0;
- }
-}
-
void Recovery() //Recovery mechanism
{
/* 洗濯物を挟む機構 */
- if((X0 > 170)&&(Y0 > 50 && Y0 < 200)) {
- mecha_power[1] = 0.6;
- }else if((X0 < 100)&&(Y0 > 50 && Y0 < 200)) {
- mecha_power[1] = -0.6;
+ if(b[5] == 1 && trigger[1] < 50){
+ mecha_power[1] -= 0.03;
+ if(mecha_power[1] < -1.0){
+ mecha_power[1] = -1.0;
+ }
+ }else if(b[5] == 0 && trigger[1] > 50){
+ mecha_power[1] += 0.03;
+ if(mecha_power[1] > 1.0){
+ mecha_power[1] = 1.0;
+ }
}else{
mecha_power[1] = 0;
}
- if((X1 > 170)&&(Y1 > 50 && Y1 < 200)) {
- mecha_power[2] = 0.6;
- }else if((X1 < 100)&&(Y1 > 50 && Y1 < 200)) {
- mecha_power[2] = -0.6;
+ if(b[4] == 1 && trigger[0] < 50){
+ mecha_power[2] += 0.03;
+ if(mecha_power[2] > 1.0){
+ mecha_power[2] = 1.0;
+ }
+ }else if(b[4] == 0 && trigger[0] > 50){
+ mecha_power[2] -= 0.03;
+ if(mecha_power[2] < -1.0){
+ mecha_power[2] = -1.0;
+ }
}else{
mecha_power[2] = 0;
}
/* 洗濯物を挟む機構の首 */
- if (b[10] == 1 && b[11] == 0) {
- mecha_power[0] = 0.3;
- } else if (b[10] == 0 && b[11] == 1) {
- mecha_power[0] = -0.3;
- } else {
- mecha_power[0] =0.0;
+ if (b[2] == 1) {
+ mecha_power[0] += 0.04;
+ if(mecha_power[0] > 0.8){
+ mecha_power[0] = 0.8;
+ }
+ }else{
+ mecha_power[0] = 0;
}
- mecha_power[3] = 0;
motor[4].setSpeed(mecha_power[0]);
- motor[5].setSpeed(mecha_power[1]);
- motor[6].setSpeed(mecha_power[2]);
- motor[7].setSpeed(mecha_power[3]);
+ motor[5].setSpeed(mecha_power[2]);
+ motor[6].setSpeed(mecha_power[1]);
}
void stop()
{
- for(int i=4;i<8;i++) motor[i].setSpeed(0);
+ for(int i=4;i<7;i++) motor[i].setSpeed(0);
}
-void Drop()
+void mechamove()
{
- if(b[4] == 1 && b[5] == 0){
- mecha_power[3] = 0.5;
- }else if(b[4] == 0 && b[5] == 1) {
- mecha_power[3] = -0.5;
+ if(towel_num == 1){
+ towel.setPoint(2000);
}else{
- mecha_power[3] = 0;
+ towel.setPoint(3650);
}
- motor[7].setSpeed(mecha_power[3]);
}
-
+
int main()
{
- sw1 = 1;
- double nowX,nowY,t;
+ sw1 = true;
+ double nowX,nowY,t,LPF[4],lastLPF[4],start_angle,now_angle,original_angle = -3.14;
+ int b2[12],b3[12],estop_num=0;
for(int i=0;i<4;i++)motor[i].braking = false;
omni.wheel[0].setRadian(PI * 1 / 4);
@@ -206,31 +138,39 @@
omni.wheel[2].setRadian(PI * 5 / 4);
omni.wheel[3].setRadian(PI * 7 / 4);
angle.setInputLimits(-6.28,6.28);
- angle.setOutputLimits(-0.4,0.4);
+ angle.setOutputLimits(-0.5,0.5);
angle.setBias(0);
angle.setMode(1);
angle.setSetPoint(0);
+ button0.countreset();
button1.countreset();
+ button3.countreset();
sensor.startReceive(28);
-
+// time_.reset();
+// time_.start();
while(1){
- LED = !LED;
+// pc.printf("%d\n\r",time_.read_ms());
+// time_.reset();
+// time_.start();
+ LED = !LED;
towel.setPulse(loli[2]);
- //PS3 value
+// PS3 value
for(int i = 0; i < 12; i++) {
b[i] = ps3.getButton(i);
- pc.printf("%2d", b[i] );
+ b3[i] = b2[i] - b[i];
+ b2[i] = b[i];
+// pc.printf("%2d", b[i] );
}
for(int i = 0; i < 4; i++) {
stick[i] = ps3.getStick(i);
- pc.printf("%4d", stick[i] );
+// pc.printf("%4d", stick[i] );
}
- X0 = stick[0];
- Y0 = stick[1];
+ X0 = (stick[0] - 127.0) / 127.0;
+ Y0 = ((stick[1] * -1) + 127.0) / 127.0;
X1 = stick[2];
Y1 = stick[3];
@@ -238,117 +178,140 @@
trigger[i] = ps3.getTrigger(i);
// pc.printf("%4d",trigger[i]);
}
-
+
//Sensor value
getSensorData();
-
+
//Count
- button1.assignvalue(b[9]);
- num = button1.getCount();
+ button0.assignvalue(b[6]);
+ button1.assignvalue(b[11]);
+ button3.assignvalue(b[1]);
+ button4.assignvalue(b[0]);
+ num = button0.getCount();
+ mecha_num = button3.getCount();
+ towel_num = button4.getCount() % 2;
+ estop_num = button1.getCount() % 2;
+
+ //estop
+ if(estop_num == 1){
+ sw1 = false;
+ }else{
+ sw1 = true;
+ }
/*【Action】*/
- switch(num){
- case 0: angle.setSetPoint(0);
- clossmove0();
- Recovery();
+ switch(num){
+ case 0: towel.allstop();
+ stop();
+ break;
+ case 1: Recovery();
+ mechamove();
towel.open();
air.solenoid1_open();
nowX = posi.getX();
nowY = posi.getY();
break;
- case 1: proto.targetXY(-3000, 0, nowX ,nowY);
- towel.allstop();
- stop();
- break;
- case 2: angle.setSetPoint(1.57);
- Drop();
- clossmove3();
- towel.allstop();
- stop();
- nowX = posi.getX();
- nowY = posi.getY();
- break;
- case 3: proto.targetXY(-2000, 3000, nowX ,nowY);
- towel.allstop();
- stop();
- if((posi.getX()>-2200 && posi.getX()<-1800) && (posi.getY()>2800 && posi.getY()<3200)){
- num = 4;
- angle.setSetPoint(0);
- }
- break;
- case 4: clossmove0();
-// Recovery();
+ //case 2: proto.targetXY(2000,3000, nowX, nowY);
+// towel.allstop();
+// stop();
+// break;
+// case 3: mechamove();
+// stop();
// nowX = posi.getX();
// nowY = posi.getY();
// break;
-// case 5: proto.targetXY(-3000, 0, nowX ,nowY);
-// break;
-// case 6: angle.setSetPoint(1.57);
-// Drop();
-// clossmove3();
-// nowX = posi.getX();
-// nowY = posi.getY();
-// break;
-// case 7: proto.targetXY(-3000,2600,-3000,0);
-// if(posi.getX()>-3200 && posi.getX()<-2800 && posi.getY()>2400 && posi.getY()<2800){
-// num = 8;
-// }
-// break;
-// case 8: angle.setSetPoint(1.57);
-// clossmove3();
-// nowX = posi.getX();
-// nowY = posi.getY();
-//
-// towel.drop();
-// t = timer.read();
-// if(b[4] == 1 && b[5] == 1){
-// timer.start();
-// }
-// if(t > 5){
-// towel.close();
-// }else if(t > 20){
-// angle.setSetPoint(0);
-// air.solenoid2_open();
-// }
-// break;
-// case 9: angle.setSetPoint(0);
-// proto.targetXY(0, 0,nowX ,nowY);
+// case 4: proto.targetXY(0, 0, nowX, nowY);
+// towel.allstop();
+// stop();
// break;
default: motor[0].setSpeed(0);
motor[1].setSpeed(0);
motor[2].setSpeed(0);
motor[3].setSpeed(0);
+ towel.allstop();
+
air.solenoid1_close();
air.solenoid2_close();
}
- angle.setProcessValue(posi.getTheta());
- spin_power = -angle.compute();
-
- if(num == 0 || num == 2 || num == 4 || num == 6 || num == 8){
- omni.computeXY(x2,y2,spin_power);
+ towel.pid_compute();
+
+ if(num == 0){
+ motor[0].setSpeed(0);
+ motor[1].setSpeed(0);
+ motor[2].setSpeed(0);
+ motor[3].setSpeed(0);
+ air.solenoid1_close();
+ air.solenoid2_close();
+
}else{
- proto.Input_nowXY(posi.getX(),posi.getY());
- proto.calculate();
- omni.computeXY(proto.getvalue_x(), proto.getvalue_y(), spin_power);
+ power = hypot(X0,Y0);
+ theta = atan2(Y0,X0) + posi.getTheta();
+ /**
+ * ここまでテンプル
+ */
+ start_angle = posi.getTheta();
+
+ if ( b3[8] == 1) original_angle -= 1.57;
+ if ( b3[9] == 1) original_angle += 1.57;
+
+ if ( b[3] ) original_angle = start_angle;
+
+ now_angle = start_angle - original_angle;
+
+ if ( now_angle > 3.14 ) {
+ now_angle -= 6.28;
+ }
+ if ( now_angle < -3.14 ) {
+ now_angle += 6.28;
+ }
+
+ angle.setProcessValue(now_angle);
+ spin_power = -angle.compute();
+ if(b[7] == 1){
+ power *= 1.0;
+ }else{
+ power *= 0.3;
+ }
+ omni.computeCircular(power,theta,spin_power);
+
+ for(int i = 0; i < 4; i++){
+ value[i] = omni.wheel[i];
+ LPF[i] = (1 - 0.8) * lastLPF[i] + 0.8 * value[i];
+ lastLPF[i] = LPF[i];
+ motor[i].setSpeed(LPF[i]);
+// pc.printf("%f ", LPF[i]);
+ }
+
+ //}else{
+// angle.setProcessValue(posi.getTheta());
+// spin_power = -angle.compute();
+// proto.Input_nowXY( posi.getX() , posi.getY() );
+// proto.calculate();
+// omni.computeXY(proto.getvalue_x(), proto.getvalue_y(), spin_power);
+//
+// for(int i = 0; i < 4; i++){
+// value[i] = omni.wheel[i];
+// motor[i].setSpeed(value[i]);
+//// pc.printf("%f ", value[i]);
+// }
+//
}
- for(int i = 0; i < 4; i++){
- value[i] = omni.wheel[i];
- motor[i].setSpeed(value[i]);
-// pc.printf("%f ", value[i]);
- }
// wait(0.001);
- pc.printf(" loli:%d case:%d",loli[2],num);
-
+// pc.printf(" loli:%d ",loli[2]);
+// pc.printf(" %d %d case:%d ",mecha_num,towel_num,num);
+// for(int i=0;i<12;i++) pc.printf(" %d ",b3[i]);
//Wheel value
+// pc.printf(" stickX:%f stickY:%f power:%f θ:%f",X0,Y0,power,theta);
// pc.printf(" vector:%f pid:%.2f",proto.vector,spin_power);
//Odmetry
-// pc.printf("x:%d y:%d θ:%f now:%f",posi.getX(),posi.getY(),posi.getTheta(),proto.nowDis);
+// pc.printf("x:%d y:%d θ:%f ",posi.getX(),posi.getY(),posi.getTheta());
- pc.printf("\r\n");
+// pc.printf("\r\n");
}
}
+
\ No newline at end of file