大季 矢花
da
Dependencies: mbed TrapezoidControl QEI
Diff: System/Process/Process.cpp
- Revision:
- 9:f93fc79a49ea
- Parent:
- 8:6fb3723f7747
- Child:
- 10:1295d39fec3a
diff -r 6fb3723f7747 -r f93fc79a49ea System/Process/Process.cpp
--- a/System/Process/Process.cpp Tue Oct 02 12:18:05 2018 +0000
+++ b/System/Process/Process.cpp Thu Oct 04 12:14:25 2018 +0000
@@ -59,19 +59,20 @@
return result;
}
-#define TIRE_FR 4 //足回り前右
-#define TIRE_FL 5 //足回り前左
+#define TIRE_FR 0 //足回り前右
+#define TIRE_FL 1 //足回り前左
#define TIRE_BR 2 //足回り後右
#define TIRE_BL 3 //足回り後左
-#define Angle_R 0 //角度調節右
-#define Angle_L 1 //角度調節左
-
+#define Angle_R 4 //角度調節右
+#define Angle_L 5 //角度調節左
#define Lim_AR 3 //角度調節右
#define Lim_AL 4 //角度調節左
#define Lim_R 0 //センター右
#define Lim_L 1 //センター左
+#define EMS_0 LimitSw::IsPressed(8)
+#define EMS_1 LimitSw::IsPressed(9)
//************メカナム********************
const int mecanum[15][15]=
@@ -136,18 +137,20 @@
Ticker Color_T;
//************ライントレース変数*******************
+
+
//ROタコン
-QEI RtX(RT11_PIN, RT12_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X2_ENCODING);
-QEI RtY(RT21_PIN, RT22_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X2_ENCODING);
+QEI RtX(RT11_PIN, RT12_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
+QEI RtY(RT21_PIN, RT22_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
Ticker get_rpm;
PID Rt_X = PID(0.03, -255, 255, 0.1, 0, 0);
PID Rt_Y = PID(0.03, -255, 255, 0.1, 0, 0);
bool Rt_flagX = false;
-bool Rt_flagY = false;
-int rpmX;
-int rpmY;
-int disX;
-int disY;
+
+double rpmX;
+double rpmY;
+double disX;
+double disY;
int palseX;
int palseY;
int RtpwmX;
@@ -161,18 +164,23 @@
//ROタコン
//************ジャイロ*******************
+
+bool Angle_flagI = false;
float Angle;
PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0);
-bool Angle_flagX = false;
-bool Angle_flagY = false;
-bool Angle_flagI = false;
+
float rotateY;
-int AngletargetX = 50;
-int AngletargetY = -50;
+int AngletargetX = 20;
+int AngletargetY = -20;
int Angle_I = -5;
//************ジャイロ*******************
-
+//************Buzzer******************
+DigitalOut buzzer(BUZZER_PIN);
+void BuzzerTimer_func();
+Ticker BuzzerTimer;
+bool Emsflag = false;
+//************Buzzer******************
#pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
#ifdef USE_SUBPROCESS
@@ -211,6 +219,7 @@
void SystemProcessInitialize()
{
#pragma region USER-DEFINED_VARIABLE_INIT
+ get_rpm.attach_us(&filip,100);
/*Replace here with the initialization code of your variables.*/
@@ -295,8 +304,8 @@
SystemProcessInitialize();
while(1)
- {
- get_rpm.attach_us(&filip,1000);
+ {
+ /*get_rpm.attach_us(&filip,1000);
disX = 48*3.141592*rpmX;
disY = 48*3.141592*rpmY;
@@ -304,10 +313,10 @@
RtpwmX = Rt_X.SetPV(disX , goalX);
RtpwmY = Rt_Y.SetPV(disY , goalY);
- //if(controller->Button.B){
+ if(controller->Button.B){
Rt_flagX = true;
- //}
- /*Rt_flagY = true;
+ }
+ Rt_flagY = true;
if (Rt_flagY){
motor[TIRE_FR].dir = SetStatus(RtpwmY);
motor[TIRE_FL].dir = SetStatus(RtpwmY);
@@ -318,14 +327,14 @@
motor[TIRE_BR].pwm = SetPWM(RtpwmY);
motor[TIRE_BL].pwm = SetPWM(RtpwmY);
}
- if(goalY - 5 < disY && disY < goalY + 5){
+ if(goalY - 15 < disY && disY < goalY + 15){
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
Rt_flagY = false;
Rt_flagX = true;
- }*/
+ }
if(Rt_flagX){
motor[TIRE_FR].dir = SetStatus(RtpwmX);
motor[TIRE_FL].dir = SetStatus(-RtpwmX);
@@ -336,7 +345,7 @@
motor[TIRE_BR].pwm = SetPWM(RtpwmX);
motor[TIRE_BL].pwm = SetPWM(RtpwmX);
}
- if(goalX - 5 < disX && disX < goalX + 5){
+ if(goalX - 15 < disX && disX < goalX + 15){
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
@@ -348,9 +357,10 @@
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
}
- pc.printf("%d \r\n",RtpwmX);
+ pc.printf("%f \r\n",RtpwmX);
wait_ms(50);
+
/*if(LimitSw::IsPressed(9)) LED_DEBUG0 = LED_ON;
else LED_DEBUG0 = LED_OFF;*/
@@ -381,6 +391,18 @@
}
}
+ if (EMS_0 || EMS_1 && !Emsflag){
+ buzzer = 1;
+ BuzzerTimer.attach(BuzzerTimer_func, 1.2);
+ Emsflag = true;
+ }
+
+ if(!EMS_0 && !EMS_1) {
+ buzzer = 0;
+ BuzzerTimer.detach();
+ Emsflag = false;
+ }
+
SystemProcessUpdate();
}
}
@@ -454,7 +476,7 @@
//wheel.getPulses()...どちらの方向にどれだけ回ったか
//pc.printf("Pulses:%07d \r\n",wheel.getPulses());
//軸が何回転したか
- //pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*2));
+ //pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
}
#endif
@@ -711,12 +733,26 @@
motor[Angle_R].dir = BRAKE;
motor[Angle_L].dir = BRAKE;
}
+ for(int i = 0;i<20;i++){
+ float y = 0;
+ y = acc[1]*1000;
+ float rotateY = (y - 305)/2.21 - 90;
+ Angle += rotateY;
+ }
+ Angle = Angle /20;
+ pc.printf("Y:%f \r\n",Angle);
}
#endif
#if USE_PROCESS_NUM>4
static void Process4()
{
+ static bool Xnopush = false;
+ static bool Ynopush = false;
+
+ static bool Angle_flagX = false;
+ static bool Angle_flagY = false;
+
if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
motor[Angle_R].dir = BRAKE;
@@ -739,12 +775,15 @@
int gyropwmX = gyro.SetPV(Angle,AngletargetX);
int gyropwmY = gyro.SetPV(Angle,AngletargetY);
- if(controller->Button.X){
+ if(controller->Button.X && !Xnopush){
Angle_flagX = true;
- }
- if(controller->Button.Y){
+ Xnopush = true;
+ }else{Xnopush = false;}
+
+ if(controller->Button.Y && !Ynopush){
Angle_flagY = true;
- }
+ Ynopush = true;
+ }else{Ynopush = false;}
if (Angle_flagX){
motor[Angle_R].dir = SetStatus(gyropwmX);
@@ -798,66 +837,60 @@
#if USE_PROCESS_NUM>5
static void Process5()
{
- /*wait(0.1);
- //RtX.getPulses();...どちらの方向にどれだけ回ったか
+ static bool nopushed = false;
+ static bool Rt_flagY = false;
+ /* wait(0.1);
+ //RtX.getPulses();//...どちらの方向にどれだけ回ったか
//RtY.getPulses();
pc.printf("Pulses:%07d \r\n",RtX.getPulses());
pc.printf("Pulses:%07d \r\n",RtY.getPulses());
//軸が何回転したか
- pc.printf("Rotate:%04.3f \r\n",(double)RtX.getPulses()/(ROTATE_PER_REVOLUTIONS*2));
+ pc.printf("Rotate:%04.3f \r\n",(double)RtX.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
*/
- /*
- get_rpm.attach_us(&filip,1000);
- disX = 48*3.141592*rpmX;
- disY = 48*3.141592*rpmY;
- RtpwmX = Rt_X.SetPV(disX , goalX);
- RtpwmY = Rt_Y.SetPV(disY , goalY);
+ if(controller->Button.B && !nopushed){
+ Rt_flagY = true;
+ nopushed = true;
+ }else{nopushed = false;}
+
- if(controller->Button.B){
- Rt_flagY = true;
- }
- if (Rt_flagY){
- motor[TIRE_FR].dir = SetStatus(RtpwmY);
+ if (Rt_flagY && SetPWM(RtpwmY) > 0){
+ filip();
+
+ motor[TIRE_FR].dir = SetStatus(-RtpwmY);
motor[TIRE_FL].dir = SetStatus(RtpwmY);
motor[TIRE_BR].dir = SetStatus(-RtpwmY);
- motor[TIRE_BL].dir = SetStatus(-RtpwmY);
+ motor[TIRE_BL].dir = SetStatus(RtpwmY);
motor[TIRE_FR].pwm = SetPWM(RtpwmY);
motor[TIRE_FL].pwm = SetPWM(RtpwmY);
motor[TIRE_BR].pwm = SetPWM(RtpwmY);
motor[TIRE_BL].pwm = SetPWM(RtpwmY);
}
- if(goalY - 15 < disY && disY < goalY + 15){
+ else if(Rt_flagY && SetPWM(RtpwmY) < 0)
+ {
+ filip();
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
- Rt_flagY = false;
- Rt_flagX = true;
}
- if(Rt_flagX){
- motor[TIRE_FR].dir = SetStatus(RtpwmX);
- motor[TIRE_FL].dir = SetStatus(-RtpwmX);
- motor[TIRE_BR].dir = SetStatus(RtpwmX);
- motor[TIRE_BL].dir = SetStatus(-RtpwmX);
- motor[TIRE_FR].pwm = SetPWM(RtpwmX);
- motor[TIRE_FL].pwm = SetPWM(RtpwmX);
- motor[TIRE_BR].pwm = SetPWM(RtpwmX);
- motor[TIRE_BL].pwm = SetPWM(RtpwmX);
- }
- if(goalX - 15 < disX && disX < goalX + 15){
+ else
+ {
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
- Rt_flagX = false;
- }else{
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- }*/
+ }
+
+ //pc.printf("RtpwmX:%f \r\n", RtpwmX);
+
+
+ pc.printf("PWM:%d \r\n", RtpwmY);
+ pc.printf("回転数:%f \r\n" ,rpmY);
+ pc.printf("距離:%f \r\n", disY);
+
+
}
#endif
@@ -1040,7 +1073,6 @@
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
}
-
*/
}
#endif
@@ -1048,7 +1080,19 @@
#if USE_PROCESS_NUM>7
static void Process7()
{
-
+ //et_rpm.attach_us(&filip,1000);
+
+ disX = 48*3.141*rpmX;
+ disY = 48*3.141*rpmY;
+
+ RtpwmX = Rt_X.SetPV(disX , goalX);
+ RtpwmY = Rt_Y.SetPV(disY , goalY);
+
+ /*pc.printf("disX:%f \r\n", disX);
+ pc.printf("disY:%f \r\n", disY);
+ pc.printf("RtpwmX:%f \r\n", RtpwmX);
+ pc.printf("RtpwmY:%f \r\n", RtpwmY);
+ wait_ms(50);*/
}
#endif
@@ -1103,8 +1147,14 @@
palseX = RtX.getPulses();
palseY = RtY.getPulses();
- rpmX = palseX/(ROTATE_PER_REVOLUTIONS*2);
- rpmY = palseY/(ROTATE_PER_REVOLUTIONS*2);
+ rpmX = (double)palseX/(ROTATE_PER_REVOLUTIONS*4);
+ rpmY = (double)palseY/(ROTATE_PER_REVOLUTIONS*4);
+
+ //disX = 48*3.141592*rpmX;
+ disY = 48*3.141*rpmY;
+
+ //RtpwmX = (int)Rt_X.SetPV(disX , goalX);
+ RtpwmY = (int)Rt_Y.SetPV(disY , goalY);
}
@@ -1147,4 +1197,7 @@
wait_us(3);
Color_D[2] = ColorIn(3);
}
+void BuzzerTimer_func() {
+ buzzer = !buzzer;
+}
#pragma endregion