Miya Miyagawa
The last version programs
Dependencies: mbed TrapezoidControl Pulse QEI
Revision 30:76b140d176f0, committed 2020-03-27
- Comitter:
- Ryosei
- Date:
- Fri Mar 27 14:03:12 2020 +0000
- Parent:
- 29:5365ee0521f6
- Commit message:
- h
Changed in this revision
| System/Process/Process.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/System/Process/Process.cpp Sun Oct 06 03:53:06 2019 +0000
+++ b/System/Process/Process.cpp Fri Mar 27 14:03:12 2020 +0000
@@ -159,7 +159,7 @@
{
double Distance=0;
double Duration=0;
- double temp=26;////////////////////////////////////////////////温度
+ double temp=10;////////////////////////////////////////////////温度
if(num==0) {
Trig0.write_us(1,10);
Duration=Echo0.read_high_us(5000);
@@ -172,7 +172,7 @@
double sspead=331.5+0.6*temp;
Distance=Duration*sspead*100/1000000;
} else {
- return 0;
+ return 0-1.5;
}
return Distance;
}
@@ -392,13 +392,14 @@
}
Ult_left=UltraRead(0);//////////////////////////////////////////left sensor
Ult_right=UltraRead(1);//////////////////////////////////////////right sensor
- for(int i=0; i<19; i++) {
- pc.printf("%d",LimitSw::IsPressed(i));
- if(i==18) {
- pc.printf("\r\n");
- }
- }
-// pc.printf("%lf,%lf\r\n",Ult_left,Ult_right);
+ //for(int i=0; i<19; i++) {
+// pc.printf("%d",LimitSw::IsPressed(i));
+// if(i==18) {
+// pc.printf("\r\n");
+// }
+// }
+//
+ pc.printf("%lf,%lf\r\n",Ult_left,Ult_right);
// pc.printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
// pc.printf("%d\n\r",current);
/*上
@@ -421,14 +422,14 @@
LedOut(6);
startFlag = false;
lock = false;
- lineFase = 0;
+ lineFase = 100;
lineCount = 0;
lineCheck = false;
countW = 0;
if(LimitSw::IsPressed(REDBLUE_SW)) {
- current = 4;
+ current = 5;
} else {
- current = 4;
+ current = 5;
}
}
buzzer.period(1.0/800);
@@ -731,17 +732,17 @@
motor[TIRE_BL].dir = FOR;
motor[TIRE_BR].dir = BACK;
motor[TIRE_FR].dir = BACK;
- if(linePara[4] >= -1 && linePara[4] <= 1) {
+ if(linePara[4] ==0) {
lineFase = 3;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_FR].dir = BRAKE;
}
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_BL].pwm = 20;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FL].pwm = 16;
+ motor[TIRE_BL].pwm = 16;
+ motor[TIRE_BR].pwm = 16;
+ motor[TIRE_FR].pwm = 16;
} else if(lineFase == 3) { // 右 ライントレース
switch(linePara[4]) {
case -2:
@@ -854,13 +855,6 @@
motor[TIRE_BL].dir = BACK;
motor[TIRE_BR].dir = BACK;
motor[TIRE_FR].dir = FOR;
- if (linePara[LINE_TOW_2] == 0) { // １と２逆になります。
- lineFase = 6;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_FR].dir = BRAKE;
- }
if(linePara[LINE_TOW_2] == 0) {
if(!LimitSw::IsPressed(SHEETS_SW)) {
lineFase=100;
@@ -926,7 +920,7 @@
motor[TIRE_FR].dir = BACK;
motor[TIRE_FR].pwm = 16;
} else {
- if(LimitSw::IsPressed(QF_SW)) {
+ if(!(LimitSw::IsPressed(QF_SW))) {
switch(linePara[LINE_TOW_2]) {
case -2:
tirePWM[TIRE_FL] = -10;
@@ -1130,7 +1124,7 @@
motor[TIRE_FR].pwm = 50;
}
} else if(lineFase == 8) { // タオル1 解放
- Air[0] = SOLENOID_ON;
+ Air[TOWEL1] = SOLENOID_ON;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
@@ -1426,7 +1420,7 @@
motor[TIRE_FR].dir = BACK;
motor[TIRE_FR].pwm = 20;
} else {
- if(LimitSw::IsPressed(QF_SW)) {
+ if(!(LimitSw::IsPressed(QF_SW))) {
switch(linePara[LINE_TOW_1]) {
case -2:
tirePWM[TIRE_FL] = -10;
@@ -1630,7 +1624,7 @@
motor[TIRE_FR].pwm = 50;
}
} else if(lineFase == 16) { // タオル2 解放
- Air[1] = SOLENOID_ON;
+ Air[TOWEL2] = SOLENOID_ON;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
@@ -2216,7 +2210,7 @@
motor[LIFT_U].dir=FOR;
motor[LIFT_U].pwm=150;
} else {
- motor[LIFT_U].dir=FOR;
+ motor[LIFT_U].dir=BRAKE;
motor[LIFT_U].pwm=150;
}
switch(linePara[3]) {
@@ -2250,7 +2244,7 @@
break;
case 1:
tirePWM[TIRE_FL] = -20;
- tirePWM[TIRE_BL] = 40;
+ tirePWM[TIRE_BL] = 30;
tirePWM[TIRE_BR] = 20;
tirePWM[TIRE_FR] = -30;
adjAnable = true;
@@ -2339,7 +2333,7 @@
motor[LIFT_U].dir=FOR;
motor[LIFT_U].pwm=150;
} else {
- motor[LIFT_U].dir=BACK;
+ motor[LIFT_U].dir=BRAKE;
motor[LIFT_U].pwm=150;
}
motor[TIRE_FL].dir = BACK;
@@ -2364,13 +2358,7 @@
motor[TIRE_BR].pwm = 16;
motor[TIRE_FR].pwm = 16;
} else if(lineFase == 21) {
- if(!(LimitSw::IsPressed(LSW_UB))) {
- motor[LIFT_U].dir=FOR;
- motor[LIFT_U].pwm=150;
- } else {
- motor[LIFT_U].dir=FOR;
- motor[LIFT_U].pwm=150;
- }
+
switch(linePara[2]) {
case -2:
tirePWM[TIRE_FL] = 0;
@@ -4218,7 +4206,6 @@
#if USE_PROCESS_NUM>7
static void Process7()
{
- static int SW_flag=0;
static int Processflag=0;
if(Processflag==0) {
if(Limitphase==0) {
@@ -4282,7 +4269,7 @@
motor[LIFT_RB].dir = BRAKE;
motor[LIFT_RB].pwm = 200;
Air[TOWEL0]=1;
- SW_flag=0;
+// SW_flag=0;
Limitphase=0;
current=0;
startFlag=true;
@@ -4305,7 +4292,7 @@
motor[LIFT_RB].dir = BRAKE;
motor[LIFT_RB].pwm = 200;
Air[TOWEL0]=1;
- SW_flag=0;
+// SW_flag=0;
Limitphase=0;
current=0;
startFlag=true;