yusuke takahashi
driver
Dependencies: HMC6352 PID mbed
Revision 1:3b61675573b1, committed 2014-01-24
- Comitter:
- yusuke_robocup
- Date:
- Fri Jan 24 06:27:03 2014 +0000
- Parent:
- 0:bde8ed56b164
- Commit message:
- driver
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| main.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r bde8ed56b164 -r 3b61675573b1 main.cpp
--- a/main.cpp Wed Jun 19 08:42:55 2013 +0000
+++ b/main.cpp Fri Jan 24 06:27:03 2014 +0000
@@ -6,13 +6,62 @@
#include "main.h"
+void whiteliner()
+{
+ static int tmp[5] = {0};
+ static int sum = 0;
+
+ sum -= tmp[4];
+ sum += whiteout_flag;
+ tmp[4] = tmp[3];
+ tmp[3] = tmp[2];
+ tmp[2] = tmp[1];
+ tmp[1] = tmp[0];
+ tmp[0] = whiteout_flag;
+
+ //return sum/5;
+}
+
+int hansya_x(int x)
+{
+ static int tmp[5] = {0};
+ static int sum = 0;
+
+ sum -= tmp[4];
+ sum += x;
+ tmp[4] = tmp[3];
+ tmp[3] = tmp[2];
+ tmp[2] = tmp[1];
+ tmp[1] = tmp[0];
+ tmp[0] = x;
+
+ return sum/5;
+}
+
+
+int hansya_y(int y)
+{
+ static int tmp[5] = {0};
+ static int sum = 0;
+
+ sum -= tmp[4];
+ sum += y;
+ tmp[4] = tmp[3];
+ tmp[3] = tmp[2];
+ tmp[2] = tmp[1];
+ tmp[1] = tmp[0];
+ tmp[0] = y;
+
+ return sum/5;
+}
+
void PidUpdate()
{
static uint8_t Fflag = 0;
pid.setSetPoint(((int)((REFERENCE + standTu) + 360) % 360) / 1.0);
- inputPID = (((int)(compass.sample() - (standard * 10.0) + 5400.0) % 3600) / 10.0);
+ inputPID = (((int)(compass.sample() - ((standard) * 10.0) + 5400.0) % 3600) / 10.0);
//pc.printf("%f\n",timer1.read());
pid.setProcessValue(inputPID);
@@ -45,7 +94,7 @@
}
}
-
+/*
void move(int vxx, int vyy, int vss)
{
double motVal[MOT_NUM] = {0};
@@ -60,6 +109,37 @@
else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW;
speed[i] = (int)motVal[i];
}
+}*/
+
+void move(int vxx, int vyy, int vss)
+{
+ double motVal[MOT_NUM] = {0};
+
+ int angle = static_cast<int>( atan2( (double)vyy, (double)vxx ) * 180/PI + 360 ) % 360;
+
+ double hosei1 = 1,hosei2 = 1,hosei3 = 1,hosei4 = 1;
+
+ if(angle == 180){
+ hosei1 = 1.3;
+ }
+
+ motVal[0] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1)*hosei1;
+ motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT2)*hosei2;
+ motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3)*hosei3;
+ motVal[3] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT4)*hosei4;
+
+ for(uint8_t i = 0 ; i < MOT_NUM ; i++){
+ if(motVal[i] > MAX_POW)motVal[i] = MAX_POW;
+ else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW;
+ speed[i] = motVal[i];
+ }
+
+ //pc.printf("speed1 = %d\n",speed[0]);
+ //pc.printf("speed2 = %d\n",speed[1]);
+ //pc.printf("speed3 = %d\n",speed[2]);
+ //pc.printf("speed4 = %d\n\n",speed[3]);
+
+ ////pc.printf("%s",StringFIN.c_str());
}
int vector(double Amp,double degree,int xy)
@@ -121,6 +201,7 @@
//driver.attach(&Rx_interrupt, Serial::RxIrq);
device2.attach(&dev_rx,Serial::RxIrq);
device2.attach(&dev_tx,Serial::TxIrq);
+ underIR.attach(&whiteliner, 0.05);
pid.setInputLimits(MINIMUM,MAXIMUM);
pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
@@ -232,333 +313,372 @@
//timer1.start();
}
+
int main()
{
int vx=0,vy=0,vs=0;
+ int x_dista = 0,y_dista = 0,x_turn = 0,y_turn = 0;
+ int state_off = NONE;
+ int direction_av = 0;
+ int direction_past = 0;
+ int past_state_off = 0;
+ int accelera_Distance = 0;
uint8_t whiteFlag = 0;
+ int save_vx = 0,save_vy = 0;
+
+ int movein = 0;
init();
- while(1){
- whiteFlag = 0;
- hold_flag = 0;
- vx = 0;
- vy = 0;
- vs = (int)compassPID;
- //vs = 0;
+ while(1) {
+
+ vx=0;
+ vy=0;
- //move(vx,vy,vs);
+ //tuning = 0;
+
+ x_dista = 0;
+ y_dista = 0;
+ x_turn = 0;
+ y_turn = 0;
+ //turn_flag = 0;
- /*********************************************************************************************************
- **********************************************************************************************************
- ********************Change state *************************************************************************
- **********************************************************************************************************
- *********************************************************************************************************/
- for(uint8_t i = 2;i < 6;i++)
+ vs = compassPID;
+
+ //direction_av = moving_ave_5point(direction);
+
+
+
+ for(uint8_t i = 0;i < 6;i++)
{
AnalogIn adcIn(adc_num[i]); /* 今回更新する赤外線の個体を呼び出す */
irDistance[i] = ((adcIn.read_u16() >> 4) - stand[i]);
- if(irDistance[i] >= 100)
+ if(irDistance[i] >= 30)
{
whiteFlag = 1;
+ movein = 1;
+ whiteout = 10;
+ }
+
+ //pc.printf("%d\n",irDistance[0]);
+ }
+
+ if(lineStateX == XNORMAL){
+ if((LEFT_SONIC < 350) && (whiteFlag)){
+ lineStateX = LEFT_OUT;
}
- //pc.printf("%d\n",irDistance[0]);
+ if((LEFT_SONIC < 350) && (RIGHT_SONIC < 100) && (whiteFlag)){
+ lineStateX = LEFT_OUT;
+ }
+
+ if((RIGHT_SONIC < 350) && (whiteFlag)){
+ lineStateX = RIGHT_OUT;
+ }
+ if((RIGHT_SONIC < 350) && (LEFT_SONIC < 100) && (whiteFlag)){
+ lineStateX = RIGHT_OUT;
+ }
+
+ }else if(lineStateX == LEFT_OUT){
+ /*
+ if((LEFT_SONIC > 450) && (!whiteFlag)){
+ lineStateX = XNORMAL;
+ whiteFlag = 0;
+ }
+ */
+ if((LEFT_SONIC > 450)){
+ lineStateX = XNORMAL;
+ whiteFlag = 0;
+ }
+ }else if(lineStateX == RIGHT_OUT){
+ /*
+ if((RIGHT_SONIC > 450) && (!whiteFlag)){
+ lineStateX = XNORMAL;
+ whiteFlag = 0;
+ }
+ */
+ if((RIGHT_SONIC > 450)){
+ lineStateX = XNORMAL;
+ whiteFlag = 0;
+ }
}
- if(lineState == NORMAL){
- if((LEFT_SONIC < 350) && (whiteFlag)){
- lineState = LEFT_OUT;
- }else if((RIGHT_SONIC < 350) && (whiteFlag)){
- lineState = RIGHT_OUT;
- }else if((FRONT_SONIC < 400) && (whiteFlag)){
- lineState = FRONT_OUT;
- }else if((BACK_SONIC < 400) && (whiteFlag)){
- lineState = BACK_OUT;
+
+ if(lineStateY == YNORMAL){
+ if((FRONT_SONIC < 400) && (whiteFlag)){
+ lineStateY = FRONT_OUT;
+ }
+ if((FRONT_SONIC < 400)&& (BACK_SONIC < 100) && (whiteFlag)){
+ lineStateY = FRONT_OUT;
+ }
+ if((BACK_SONIC < 400) && (whiteFlag)){
+ lineStateY = BACK_OUT;
+ }
+ if((BACK_SONIC < 400) && (FRONT_SONIC < 100) && (whiteFlag)){
+ lineStateY = BACK_OUT;
}
- }else if(lineState == LEFT_OUT){
- if((LEFT_SONIC > 450) && (!whiteFlag)){
- lineState = NORMAL;
+ }else if(lineStateY == FRONT_OUT){
+ /*
+ if((FRONT_SONIC > 500) && (!whiteFlag)){
+ lineStateY = YNORMAL;
+ whiteFlag = 0;
}
- }else if(lineState == RIGHT_OUT){
- if((RIGHT_SONIC > 450) && (!whiteFlag)){
- lineState = NORMAL;
+ */
+ if((FRONT_SONIC > 500)){
+ lineStateY = YNORMAL;
+ whiteFlag = 0;
}
- }else if(lineState == FRONT_OUT){
- if((FRONT_SONIC > 500) && (!whiteFlag)){
- lineState = NORMAL;
+ }else if(lineStateY == BACK_OUT){
+ /*
+ if((BACK_SONIC > 500) && (!whiteFlag)){
+ lineStateY = YNORMAL;
+ whiteFlag = 0;
}
- }else if(lineState == BACK_OUT){
- if((BACK_SONIC > 500) && (!whiteFlag)){
- lineState = NORMAL;
+ */
+ if((BACK_SONIC > 500)){
+ lineStateY = YNORMAL;
+ whiteFlag = 0;
}
}
- if(state == HOLD){
- if(FRONT_SONIC < 100)hold_flag = 1;
-
- if(Distance > 140){ //審判のボール持ち上げ判定
- state = HOME_WAIT;
- }else if(!((direction == 0) || (direction == 15) || (direction == 1) || (direction == 14) || (direction == 2))){//ボールを見失った
- state = DIFFENCE;
- }else if(!(Distance <= 40)){//ボールを見失った
- state = DIFFENCE;
- }
+ if((state_off == ATTACK)&&(Distance == 10)){
+ state = 1;
}else{
- standTu = 0;
- if(state == DIFFENCE){
- if((direction == 0) && (Distance <= 20) && (IR_found) && (!xbee)){
- state = HOLD;
- }else if((Distance < 180) && (IR_found) && (!xbee)){
- state = DIFFENCE;
- }else{
- if((direction == 4) || (direction == 6) || (direction == 8) || (direction == 10)|| (direction == 12)){
- if((IR_found) && (!xbee)){
- state = DIFFENCE;
- }
- }else if((diff > 15) && (!xbee) && (IR_found)){
- state = DIFFENCE;
- }else{
- state = HOME_WAIT;
- }
- }
-
- }else{
- if((direction == 0) && (Distance <= 30) && (IR_found) && (!xbee)){
- state = HOLD;
- }else if((Distance <= 150) && (IR_found) && (!xbee)){
- state = DIFFENCE;
- }else{
- if((direction == 4) || (direction == 6) || (direction == 8) || (direction == 10)|| (direction == 12)){
- if((IR_found) && (!xbee)){
- state = DIFFENCE;
- }
- }else if((diff > 15) && (!xbee) && (IR_found)){
- state = DIFFENCE;
- }else{
- state = HOME_WAIT;
- }
- }
+ state = 0;
+ }
+
+ if(((direction == 0)||(direction == 1)||(direction == 15))){
+ state_off = ATTACK;
+ }
+ if(((direction != 0)&&(direction != 1)&&(direction != 15)&&(direction != 2)&&(direction != 14))&&(Distance <= 90)){
+ if((past_state_off != SNAKE)){
+ accelera_Distance = Distance;
}
+ state_off = SNAKE;
}
- /**/
- /*********************************************************************************************************
- **********************************************************************************************************
- ********************Change state *************************************************************************
- **********************************************************************************************************
- *********************************************************************************************************/
+ if(Distance >= 120){
+ state_off = SEARCH;
+ }
+
+ past_state_off = state_off;
- //state = HOME_WAIT;
- if(state == HOME_WAIT){
- mbedleds = 1;
- /*
- if(((RIGHT_SONIC + LEFT_SONIC) < 1100.0) && ((RIGHT_SONIC + LEFT_SONIC) > 850.0)){
- if((LEFT_SONIC > 425.0) && (RIGHT_SONIC > 425.0)){
- vx = 0;
- }else if(RIGHT_SONIC < 425.0){
- vx = (int)((RIGHT_SONIC - 425.0) * 0.02 - 10.0);
- if(vx < -15)vx = -15;
- }else if(LEFT_SONIC < 425.0){
- vx = (int)((425.0 - LEFT_SONIC ) * 0.02 + 10.0);
- if(vx > 15)vx = 15;
- }
-
- if((RIGHT_SONIC < 330.0) || (LEFT_SONIC < 330.0)){
- if((BACK_SONIC > 15.0) && (BACK_SONIC < 45.0)){
- vy = 0;
- }else if((BACK_SONIC <= 15.0) || (BACK_SONIC == PING_ERROR)){
- vy = 5;
- }else{
- vy = (int)(0.015 * (30.0 - BACK_SONIC) - 4);
- if(vy < -10)vy = -10;
+ if(IR_found){
+ if(state_off == SNAKE){
+ if((Distance == 30)||(Distance == 90)){
+ x_dista = 12*ball_sankaku[direction][0];
+ y_dista = 12*ball_sankaku[direction][1];
+
+ x_turn = 18*(turn_sankaku[direction][0]);
+ y_turn = 18*(turn_sankaku[direction][1]);
+
+ if((direction == 2)||(direction == 14)){
+ //x_turn *= 0.7;
+ y_turn *= 0.7;
}
- }else{
- if((BACK_SONIC > 95.0) && (BACK_SONIC < 125.0)){
+
+ if((direction == 2)||(direction == 14)||(direction == 1)||(direction == 15)||(direction == 0)){
+ x_turn = 7*(turn_sankaku[direction][0]);
+ y_turn = 7*(turn_sankaku[direction][1]);
+
+ x_dista = 15*ball_sankaku[direction][0];
+ y_dista = 10*ball_sankaku[direction][1];
+
+ }
+
+ if(direction == 1){
+ vx = 15;
vy = 0;
- }else if((BACK_SONIC <= 95.0) || (BACK_SONIC == PING_ERROR)){
- vy = 5;
- }else{
- vy = (int)(0.015 * (110.0 - BACK_SONIC) - 4);
- if(vy < -10)vy = -10;
}
- }
- }else if((RIGHT_SONIC + LEFT_SONIC) <= 850.0){
- if(BACK_SONIC < 200.0){
- if(RIGHT_SONIC > LEFT_SONIC){
- vx = 15;
- vy = 5;
- }else{
+
+ if(direction == 15){
vx = -15;
- vy = 5;
+ vy = 0;
}
- }else{
- vx = 0;
- vy = -10;
- }
- }else{
- if(BACK_SONIC > 500.0){
- if(RIGHT_SONIC > LEFT_SONIC){
- vx = 10;
+
+ if(direction == 2){
+ vx = 20;
vy = -10;
- }else{
- vx = -10;
+ }
+
+ if(direction == 14){
+ vx = -20;
vy = -10;
}
+
}
- }
- */
- }else if(state == DIFFENCE){
- mbedleds = 4;
- if((direction == 6) || (direction == 4)){
- vx = 20;
-
- if(LEFT_SONIC < 500){
- if((BACK_SONIC > 370) && (BACK_SONIC < 400)){
- vy = 0;
- }else if((BACK_SONIC <= 370) || (BACK_SONIC == PING_ERROR)){
- vy = 5;
- }else{
- vy = (int)(0.015 * (400.0 - BACK_SONIC) - 4);
- if(vy < -15)vy = -15;
+
+ if(Distance == 10){
+ x_dista = 8*(-ball_sankaku[direction][0]);
+ y_dista = 8*(-ball_sankaku[direction][1]);
+
+ x_turn = 22*(turn_sankaku[direction][0]);
+ y_turn = 22*(turn_sankaku[direction][1]);
+
+
+ if((direction == 2)||(direction == 14)){
+ y_turn *= 0.7;
}
- }else if(RIGHT_SONIC < 500){
- vx = 15;
- vy = -10;
- }else{
- if((BACK_SONIC > 70) && (BACK_SONIC < 100)){
- vy = 0;
- }else if((BACK_SONIC <= 70) || (BACK_SONIC == PING_ERROR)){
- vy = 5;
- }else{
- vy = (int)(0.015 * (100.0 - BACK_SONIC) - 4);
- if(vy < -15)vy = -15;
+
+ if(direction == 2){
+ vx = 20;
+ vy = -15;
}
+ if(direction == 14){
+ vx = -20;
+ vy = -15;
+ }
}
- }else if((direction == 10) || (direction == 12)){
- vx = -20;
-
- if(RIGHT_SONIC < 500){
- if((BACK_SONIC > 370) && (BACK_SONIC < 400)){
- vy = 0;
- }else if((BACK_SONIC <= 370) || (BACK_SONIC == PING_ERROR)){
- vy = 5;
- }else{
- vy = (int)(0.015 * (400.0 - BACK_SONIC) - 4);
- if(vy < -15)vy = -15;
- }
- }else if(LEFT_SONIC < 500){
- vx = -15;
- vy = -10;
- }else{
- if((BACK_SONIC > 70) && (BACK_SONIC < 100)){
- vy = 0;
- }else if((BACK_SONIC <= 70) || (BACK_SONIC == PING_ERROR)){
- vy = 5;
- }else{
- vy = (int)(0.015 * (100.0 - BACK_SONIC) - 4);
- if(vy < -15)vy = -15;
- }
+
+ if((direction == 2)||(direction == 14)||(direction == 1)||(direction == 15)||(direction == 0)){
+ x_dista = 0;
+ y_dista = 0;
}
- }else if(direction == 8){
-
- if(LEFT_SONIC > RIGHT_SONIC){
- vx = -20;
- }else{
+ if(direction == 2){
vx = 20;
+ vy = -15;
}
- if((RIGHT_SONIC < 500) || (LEFT_SONIC < 500)){
- if(BACK_SONIC < 500){
- vy = -4;
- }else{
- vy = (int)(0.015 * (500.0 - BACK_SONIC) - 4);
- if(vy < -15)vy = -15;
- }
- }else{
- if(BACK_SONIC < 200){
- vy = -4;
- }else{
- vy = (int)(0.015 * (200.0 - BACK_SONIC) - 4);
- if(vy < -15)vy = -15;
- }
+ if(direction == 14){
+ vx = -20;
+ vy = -15;
+ }
+
+ vx = x_turn + x_dista;
+ vy = y_turn + y_dista;
+
+ /*
+ if((accelera_Distance == 90)){
+ if(Distance == 10){
+ vx *= 0.3;
+ vy *= 0.3;
+ }
+
+ if(Distance == 30){
+ vx *= 0.4;
+ vy *= 0.4;
+ }
+ }*/
+ /*
+ if((accelera_Distance == 10)){
+ if((direction == 4)||(direction == 12)){
+ vx = 0;
+ vy = -10;
+ }
+ }*/
+
+ }else if(state_off == ATTACK){
+ if(direction == 0){
+ vx = 10*ball_sankaku[direction][0];
+ vy =20*ball_sankaku[direction][1];
+ /*
+ if(ultrasonicVal[1] < 380){
+ vy = 10;
+ vx = -20;
+ }
+
+ if(ultrasonicVal[3] < 380){
+ vy = 10;
+ vx = 20;
+ } */
+ }
+ if(direction == 1){
+ vx = 15*1.3;
+ vy = 20*1.3;
+ }
+ if(direction == 15){
+ vx = -15*1.3;
+ vy = 20*1.3;
+ }
+ if(direction == 2){
+ vx = 25;
+ vy = 0;
+ }
+ if(direction == 14){
+ vx = -25;
+ vy = 0;
}
- }else if(direction == 2){
-
- vx = 25;
- vy = 0; //0
+ if(Distance > 30){
+ if(direction == 2){
+ vx = 20;
+ vy = 10;
+ }
+ if(direction == 14){
+ vx = -20;
+ vy = 10;
+ }
+ }
- }else if(direction == 14){
-
- vx = -25;
- vy = 0; //-4
+ }else if(state_off == SEARCH){
+ vx = 24*ball_sankaku[direction][0];
+ vy = 24*ball_sankaku[direction][1];
- }else if(direction == 1){
+ if(direction == 2){
+ vx = 20;
+ }
+ if(direction == 14){
+ vx = -20;
+ }
+ if(direction == 0){
+ vx = 20*ball_sankaku[direction][0];
+ vy = 15*ball_sankaku[direction][1];
+ }
+ if(direction == 1){
+ vx = 20*ball_sankaku[direction][0];
+ vy = 10*ball_sankaku[direction][1];
+ }
+ if(direction == 15){
+ vx = 20*ball_sankaku[direction][0];
+ vy = 10*ball_sankaku[direction][1];
+ }
+ if(direction == 4){
+ vx = 0;
+ vy = -15;
+ }
+ if(direction == 12){
+ vx = 0;
+ vy = -15;
+ }
+ }
-
+ if(direction == 2){
vx = 20;
- vy = 0; //0
-
-
- }else if(direction == 15){
-
+ vy = 0;
+
+ }
+ if(direction == 14){
vx = -20;
- vy = 0; //-3
-
- }else if(direction == 0){
-
- vx = 0;
- vy = 20;
-
- }else{//error
-
- vx = 0;
vy = 0;
-
- }
- }else if(state == HOLD){
- mbedleds = 15;
-
- vy = 20;
-
- if(((RIGHT_SONIC + LEFT_SONIC) < 1800.0) && ((RIGHT_SONIC + LEFT_SONIC) > 1400.0)){
- standTu = (RIGHT_SONIC - LEFT_SONIC) / 25.0;
- }
+ }
+
+ }else{
+ vx = 0;
+ vy = 0;
}
- if(lineState == NORMAL){
- //mbedleds = 1;
-
- }else if(lineState == LEFT_OUT){
- //mbedleds = 2;
-
- vx = 30;
- }else if(lineState == RIGHT_OUT){
- //mbedleds = 4;
-
- vx = -30;
- }else if(lineState == FRONT_OUT){
- //mbedleds = 8;
-
- vy = -40;
- }else if(lineState == BACK_OUT){
- //mbedleds = 12;
-
- vy = 40;
+ vx *= 1.3* 0.8;
+ vy *= 0.7 * 0.8;
+
+
+
+ if(lineStateX == LEFT_OUT){
+ vx += 20;
+ }else if(lineStateX == RIGHT_OUT){
+ vx -= 20;
}
- //vx = vector(10,45,X);
- //vy = vector(10,45,Y);
- //vx = 40;
- //vy = 0;
- //pc.printf("%d\t%d\n",vx,vy);
- //vy = -3;
- //vs = 0;
- //vx = 0;
- //vx = 10;
- //vx = 25;
- //vy = 0;
+ if(lineStateY == FRONT_OUT){
+ vy -= 15;
+ }else if(lineStateY == BACK_OUT){
+ vy += 15;
+ }
+
+ //vx *= 0.8;
+ //vy *= 0.8;
+
+ direction_past = direction;
+
move(vx,vy,vs);
}
-}
\ No newline at end of file
+}
diff -r bde8ed56b164 -r 3b61675573b1 main.h
--- a/main.h Wed Jun 19 08:42:55 2013 +0000
+++ b/main.h Fri Jan 24 06:27:03 2014 +0000
@@ -29,9 +29,9 @@
#define PID_CYCLE 0.06 //s
-#define P_GAIN 0.75 //0.78
+#define P_GAIN 0.65 //0.78
#define I_GAIN 0.0 //0.0
-#define D_GAIN 0.006 //0.009
+#define D_GAIN 0.009 //0.009
#define OUT_LIMIT 30.0
#define MAX_POW 100
@@ -53,14 +53,20 @@
PID pid(P_GAIN,I_GAIN,D_GAIN, RATE); //battery is low power version 30.0 0.58 1.0 0.015 battery is high power version 30.0 0.42, 1.0, 0.013 power is low but perfect 20.0 0.45, 0.0, 0.010
Ticker pidUpdata;
Ticker irDistanceUpdata;
+Ticker underIR;
Timer timer1;
Timer Survtimer;
+Timer lasttimer;
LocalFileSystem local("local"); // マウントポイントを定義（ディレクトリパスになる）
enum{
- NORMAL,
+ XNORMAL,
LEFT_OUT,
RIGHT_OUT,
+};
+
+enum{
+ YNORMAL,
FRONT_OUT,
BACK_OUT,
};
@@ -72,6 +78,13 @@
HOLD,
};
+enum{
+ NONE,
+ ATTACK,
+ SNAKE,
+ SEARCH
+};
+
PinName adc_num[6] = {
p15,
p16,
@@ -80,16 +93,25 @@
p19,
p20,
};
+
+int under_val[6] = {0};
+
+int whiteout_flag = 0;
+
double standTu = 0;
int speed[MOT_NUM] = {0};
uint8_t hold_flag = 0;
uint8_t state = HOME_WAIT;
-uint8_t lineState = NORMAL;
+uint8_t lineStateY = YNORMAL;
+uint8_t lineStateX = XNORMAL;
double inputPID = 180.0;
static double standard;
double compassPID = 0.0;
+int whiteout;
+int whitevalue;
+
extern int diff;
extern string StringFIN;
@@ -116,3 +138,44 @@
#define LEFT_SONIC ultrasonicVal[3]
+double ball_sankaku[16][2] = {
+ {0 , 1 },
+ {0.390 , 0.920},
+ {0.866 , 0.500},
+ {0.927 , 0.374},
+ {1 , 0 },
+ {0.920 ,-0.390},
+ {0.707 ,-0.707},
+ {0.374 ,-0.927},
+ {0 ,-1 },
+ {-0.390,-0.920},
+ {-0.707,-0.707},
+ {-0.927,-0.374},
+ {-1 , 0 },
+ {-0.920, 0.390},
+ {-0.866, 0.500},
+ {-0.374, 0.927}
+};
+
+double turn_sankaku[16][2] = {
+ { 0 ,0 },
+ /*{ 1 ,0 },*/{ 0.920,-0.390 },
+ { 0.707,-0.707 }, //{ 0.500,-0.866 },
+ { 0.374,-0.927 },
+ { 0 ,-0.8 },
+ {-0.390,-0.920 },
+ {-0.707,-0.707 },
+ {-0.927,-0.374 },
+ {-0.927,-0.374 },
+ {0.920 ,-0.390 },
+ {0.707 ,-0.707 },
+ {0.374 ,-0.927 },
+ {0 ,-0.8 },
+ {-0.390,-0.920 },
+ {-0.707,-0.707 }, //{-0.500,-0.866 },
+ /*{-1 ,0 }*/{-0.927,-0.374 }
+};
+
+
+
+