NagaokaRoboticsClub_mbedTeam
use for experiment before the demonstration at open-campus
Dependencies: FEP ikarashiMDC omni PID R1370
Fork of
omni_sample
by 
NagaokaRoboticsClub_mbedTeam
Revision 15:d4ff132a616d, committed 2017-09-14
- Comitter:
- tkNara
- Date:
- Thu Sep 14 18:22:29 2017 +0900
- Parent:
- 14:1d9ae3128002
- Child:
- 16:7eaf3343f45e
- Commit message:
- a
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| pin_config.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Sep 05 00:41:30 2017 +0000
+++ b/main.cpp Thu Sep 14 18:22:29 2017 +0900
@@ -1,5 +1,5 @@
+#include "omni.h"
#include "mbed.h"
-#include "omni.h"
#include "ikarashiMDC.h"
#include "pin_config.h"
#include "FEP.h"
@@ -7,23 +7,22 @@
#include "PID.h"
#include "R1370.h"
-#define DEBUG
+#define DEBUG
-#define KP 2.5
-#define KI 0
-#define KD 0.05
+#define KP 0.0//1.5 //2.5
+#define KI 0.0 //0s
+#define KD 0.0 //0.05
#define RATE 0.01
Omni omni(4);
Serial RS485(RS485_TX,RS485_RX,38400);
Serial pc(USBTX,USBRX,115200);
Controller con(FEP_TX,FEP_RX,200);
DigitalOut RS485control(PA_4);
-//DigitalOut leds[4] = {PC_13,PC_14,PC_15,PA_0};
-DigitalOut attack[2] = {PA_10,PC_4};
-DigitalOut angle[2] = {PB_3,PB_5};
+DigitalOut attack[2] = {air1_0,air1_1};
+DigitalOut angle[2] = {air2_0,air2_1};
DigitalOut powerSw(powerSW);
R1370 R1370(R1370_TX,R1370_RX);
-PID pid(KP,KI,KD,RATE);
+PID pid(KP,KI,KD,RATE);
ikarashiMDC wheels[] {
ikarashiMDC(&RS485control,0,0,SM,&RS485),
ikarashiMDC(&RS485control,1,3,SM,&RS485),
@@ -40,11 +39,8 @@
for(i = 0;i < 4;i++) {
wheels[i].braking = true;
}
+ omni.setWheelRadian(PI/4,3*PI/4,5*PI/4,7*PI/4);
lift[0].braking = true;
- omni.setWheelRadian(PI/4,3*PI/4,5*PI/4,7*PI/4);
- for(i = 0;i<4;i++) {
- //leds[i] = 0;
- }
}
void pidInit()
{
@@ -55,99 +51,98 @@
pid.setSetPoint(0.0);
pid.setBias(0.0);
}
-void AllActuatorStop()
+void gyroInit()
{
-#ifdef DEBUG
- pc.printf("All actuators stop\n");
-#endif
- for(int i=0;i<1;i++)
- {
- attack[i]=0;
- angle[i]=0;
- }
+ R1370.update();
+ R1370.setZeroPoint(R1370.getAngle());
}
int main()
-{
+{
bool airFlag1 = 0,airFlag2 = 0,airStatus1 = 0,airStatus2 = 0,pidflag = 0;
int error_val = 0,i = 0;
- double pwm = 0.0;
+ float nowAngle = 0;//,oldAngle = 0;
init();
pidInit();
- while(1)
+ gyroInit();
+ while(1)
{
- if((con.receiveState()==0)&&(R1370.update()==0))
+ if(con.receiveState()==0)
{
- if(con.getButton1(6)==0) {
- powerSw = 0;
- } else {
- powerSw = 1;
- }
- error_val = 0;
- pc.printf("%lf %lf ",R1370.getAngle(),-1*pid.compute());
- if((con.getStick(0)==0)&&(pidflag==1)) {
- pidflag = 0;
- pid.setSetPoint(R1370.getAngle());;
+ if(R1370.update()==0)
+ {
+ if(con.getButton1(6)==0) {
+ powerSw = 0;
+ } else {
+ powerSw = 1;
+ }
+ error_val = 0;
+ nowAngle = R1370.getAngle();
+ if((con.getStick(0)==0)&&(pidflag==1)) {
+ pc.printf("zeroPointSet\n");
+ pidflag = 0;
+ R1370.setZeroPoint(nowAngle);
+ }else if(con.getStick(0)!=0) {
+ pidflag = 1;
+ omni.computeXY(-1*con.getStick(2),con.getStick(3),(-1*con.getStick(0))/3.0);
+ }else if((con.getStick(0)==0)&&(pidflag==0)) {
+ pid.setProcessValue(R1370.getDeviation(nowAngle));
+ omni.computeXY(-1*con.getStick(2),con.getStick(3),-1*pid.compute());
+ }
+ pc.printf("Now%lf:D%lf:set%lf:PID%lf ",R1370.getAngle(),R1370.getDeviation(nowAngle),R1370.getZeroPoint(),-1*pid.compute());
+ for(i = 0; i < 4; i++) {
+ pc.printf("%lf,",omni.getOutput(i));
+ wheels[i].setSpeed(omni.getOutput(i));
+ }
+ pc.printf("\n");
+ //昇降
+ lift[0].setSpeed(con.getStick(1));
+ //アーム攻撃(toggle)
+ pc.printf("%d\n",con.getButton2(1));
+ if((con.getButton2(1)==0)&&(airFlag1 == 0))
+ {
+ if(airStatus1==1) {
+ attack[0]=0;
+ attack[1]=1;
+ airFlag1=1;
+ airStatus1=0;
+ }else if(airStatus1==0) {
+ attack[0]=1;
+ attack[1]=0;
+ airFlag1=1;
+ airStatus1=1;
+ }
+ }else if(con.getButton2(1)==1){
+ airFlag1=0;
+ attack[0]=0;
+ attack[1]=0;
+ }
+ //アーム角度(toggle)
+ if((con.getButton2(3)==0)&&(airFlag2 == 0))
+ {
+ if(airStatus2==1) {
+ angle[0]=0;
+ angle[1]=1;
+ airFlag2=1;
+ airStatus2=0;
+ }else if(airStatus2==0) {
+ angle[0]=1;
+ angle[1]=0;
+ airFlag2=1;
+ airStatus2=1;
+ }
+ }else if(con.getButton2(3)==1){
+ airFlag2=0;
+ angle[0]=0;
+ angle[1]=0;
+ }
}else {
- pidflag = 1;
- omni.computeXY(-1*con.getStick(2),con.getStick(3),con.getStick(0));
- }
- pid.setProcessValue(R1370.getAngle());
- for(int i = 0; i < 4; i++) {
- pc.printf("%lf,",omni.getOutput(i));
- wheels[i].setSpeed(omni.getOutput(i));
- }
- pc.printf("\n");
- //昇降
- if(con.getButton1(0) == 1 && con.getButton1(1) == 0) {
- pwm = -0.9;
- } else if(con.getButton1(0) == 0 && con.getButton1(1) == 1) {
- pwm = 0.9;
- } else {
- pwm = 0.0;
- }
- lift[0].setSpeed(pwm);
- //アーム攻撃(toggle)
- if((con.getButton2(1)==0)&&(airFlag1 == 0))
- {
- if(airStatus1==1) {
- attack[0]=0;
- attack[1]=1;
- airFlag1=1;
- airStatus1=0;
- }else if(airStatus1==0) {
- attack[0]=1;
- attack[1]=0;
- airFlag1=1;
- airStatus1=1;
- }
- }else if(con.getButton2(1)==1){
- airFlag1=0;
- attack[0]=0;
- attack[1]=0;
- }
- //アーム角度(toggle)
- if((con.getButton2(3)==0)&&(airFlag2 == 0))
- {
- if(airStatus2==1) {
- angle[0]=0;
- angle[1]=1;
- airFlag2=1;
- airStatus2=0;
- }else if(airStatus2==0) {
- angle[0]=1;
- angle[1]=0;
- airFlag2=1;
- airStatus2=1;
- }
- }else if(con.getButton2(3)==1){
- airFlag2=0;
- angle[0]=0;
- angle[1]=0;
+ //pc.printf("gyro_error\n");
}
}else {
+ //pc.printf("FEP_error\n");
error_val++;
- if(error_val > 10) powerSw = 0;
+ if(error_val > 5) powerSw = 0;
+ else powerSw = 1;
}
- wait(RATE);
}
}
--- a/pin_config.h Tue Sep 05 00:41:30 2017 +0000 +++ b/pin_config.h Thu Sep 14 18:22:29 2017 +0900 @@ -9,5 +9,9 @@ static PinName const FEP_RX = PA_1; static PinName const R1370_TX = PC_12; static PinName const R1370_RX = PD_2; -static PinName const powerSW = PC_3; +static PinName const air1_0 = PB_5; +static PinName const air1_1 = PB_14; +static PinName const air2_0 = PB_3; +static PinName const air2_1 = PB_13; +static PinName const powerSW = PC_8; #endif