yusuke takahashi
driver
Dependencies: HMC6352 PID mbed
Diff: main.cpp
- Revision:
- 1:3b61675573b1
- Parent:
- 0:bde8ed56b164
--- 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
+}