CatPot 2015-2016
version 3 通信方式，マイコン等に変更あり
Dependencies: AQM0802A PID Servo mbed
Diff: main.cpp
- Revision:
- 2:e84bb87eea71
- Parent:
- 1:f91d53098d57
- Child:
- 3:4a39486ff238
--- a/main.cpp Fri Mar 06 02:15:10 2015 +0000
+++ b/main.cpp Tue Mar 10 08:28:15 2015 +0000
@@ -39,9 +39,9 @@
#include "mbed.h"
#include <math.h>
#include <sstream>
+#include "MultiSerial.h"
#include "PID.h"
#include "AQM0802A.h"
-#include "MultiSerial.h"
#include "Servo.h"
#include "main.h"
@@ -56,7 +56,7 @@
PingData[3] = rx_data[6];
Compass = rx_data[7]+rx_data[8];
- if(IrData[0] == 255){
+ if((IrData[0] == 255)||(IrData[1] == 255)||(IrData[2] == 255)){
IrNum = 12;
return;
}
@@ -65,13 +65,13 @@
}
-void move(int vr,int vl, double vs ,double Rad){
+void move(int vr,int vl, double vs ,int Degree){
double pwm[4] = {0};
uint8_t i = 0;
pwm[0] = vr - vs;
pwm[1] = 0;
pwm[2] = 0;
- pwm[3] = vl*1.2 + vs;
+ pwm[3] = vl*1.1 + vs;
for(i = 0; i < 4; i++){
if(pwm[i] > 100){
@@ -81,153 +81,275 @@
}
speed[i] = pwm[i];
}
- SetRad = Rad;
+ if(Degree > 110){
+ Degree = 110;
+ }else if(Degree < -110){
+ Degree = -110;
+ }
+ SetDegree = Degree;
+ S555.position(SetDegree);
wait_ms(10);
}
-void fool (int *Rad, int *Power){
- static int Last_Rad = 0;
+
+uint8_t PingChange(uint8_t LineData){
+ static uint8_t Last_Line;
+ static uint8_t Last_Ping;
+ uint8_t LinePing = 0;
+
+ if(!LineData){
+ return 0;
+ }
+
+ if(PingData[0] <40) LinePing = LinePing + 1;
+ if(PingData[1] <40) LinePing = LinePing + 2;
+ if(PingData[2] <40) LinePing = LinePing + 4;
+ if(PingData[3] <40) LinePing = LinePing + 8;
+
+
+ if(LinePing&0x01){
+ if((LineData&0x01) ||(Last_Line&0x01)||(Last_Ping&0x01)){
+ Last_Ping = LinePing;
+ Last_Line = LineData;
+ return 1;
+ }
+ }
+ if(LinePing&0x02){
+ if((LineData&0x02) ||(Last_Line&0x02)||(Last_Ping&0x02)){
+ Last_Ping = LinePing;
+ Last_Line = LineData;
+ return 2;
+ }
+ }
+ if(LinePing&0x04){
+ if((LineData&0x04) ||(Last_Line&0x04)||(Last_Ping&0x04)){
+ Last_Ping = LinePing;
+ Last_Line = LineData;
+ return 4;
+ }
+ }
+ if(LinePing&0x08){
+ if((LineData&0x08) ||(Last_Line&0x08)||(Last_Ping&0x08)){
+ Last_Ping = LinePing;
+ Last_Line = LineData;
+ return 8;
+ }
+ }
+ Last_Ping = 0;
+ Last_Line = 0;
+ return 0;
+
+}
+
+void fool (int *Degree, int *Power){
+ static int Last_Degree = 0;
static int Last_Vector = 1;
- int rad = *Rad;
+ int degree = *Degree;
int Temp;
- Temp = Last_Rad % 180;
-
- if((Temp>70) &&(Temp<110)){
- Temp = *Rad % 180;
- if((Temp>70) &&(Temp<110)){
- Temp = abs(*Rad - Last_Rad);
+
+
+ if((*Degree <0)||(*Degree >=360)){
+ *Degree = 0;
+ Last_Degree = 0;
+ Last_Vector = 1;
+ return ;
+ }
+
+ Temp = Last_Degree % 180;
+ if((Temp==90)){
+ Temp = *Degree % 180;
+ if((Temp==90)){
+ Temp = abs(*Degree - Last_Degree);
if(Temp>160){
Last_Vector = -1 * Last_Vector;//正転逆転切り替え
- if(*Rad/180){
- *Rad = Angle[Last_Rad%180] -(Last_Rad - *Rad%180);
+ if(*Degree/180){
+ *Degree = Angle[Last_Degree%180] -(Last_Degree - *Degree%180);
*Power = *Power * Last_Vector;
}else{
- *Rad = Angle[Last_Rad%180] -(Last_Rad%180 - *Rad);
+ *Degree = Angle[Last_Degree%180] -(Last_Degree%180 - *Degree);
*Power = *Power * Last_Vector;
}
- Last_Rad = rad;
+ Last_Degree = degree;
+ if((*Degree <= -120)||(*Degree >=120)){
+ *Degree = 0;
+ }
return;
}else if((Last_Vector+2) == 1){
/*逆転のまま角度拡張*/
- if(*Rad/180){
- *Rad = -360 + *Rad ;
- *Power = *Power * Last_Vector;
- }else{
- *Power = *Power * Last_Vector;
+ if(*Degree/180){
+ *Degree = -360 + *Degree ;
+
}
- Last_Rad = rad;
+ *Power = *Power * Last_Vector;
+ Last_Degree = degree;
+ if((*Degree <= -120)||(*Degree >=120)){
+ *Degree = 0;
+
+ }
return;
}else if((Last_Vector+2) == 3){
/*正転のまま*/
- if(*Rad/180){
- *Rad = -360 + *Rad ;
- *Power = *Power * Last_Vector;
- }else{
- *Power = *Power * Last_Vector;
- Last_Rad = rad;
+ if(*Degree/180){
+ *Degree = -360 + *Degree ;
}
- Last_Rad = rad;
+ *Power = *Power * Last_Vector;
+ Last_Degree = degree;
+ if((*Degree <= -120)||(*Degree >=120)){
+ *Degree = 0;
+ }
return;
}
}
}
/*通常動作*/
- Last_Vector = RadToVector[(*Rad-1)/90];
- *Rad = Angle[*Rad%180];
+ if(*Degree == 0){
+ Last_Vector = DegreeToVector[0];
+ *Degree = Angle[*Degree%180];
+ *Power = *Power * Last_Vector;
+ Last_Degree = degree;
+ return ;
+ }
+ Last_Vector = DegreeToVector[(*Degree-1)/90];
+ *Degree = Angle[*Degree%180];
*Power = *Power * Last_Vector;
- Last_Rad = rad;
+ Last_Degree = degree;
+ if((*Degree <= -120)||(*Degree >=120)){
+ *Degree = 0;
+ }
}
-int IrRad(){
+int IrDegree(){
/*irの1位の値，2位の場所からirの360へ持っていく*/
- uint8_t IrF =IrData[0]/12,IrS = IrData[0]%12;
- int irrad;
- irrad = 359 - IrF*30;
-
- if(IrS){
- if(IrF == 0 ){
- if(IrS == 11){
- irrad = 15;
+ uint8_t IrF ,IrS;
+ unsigned int irdegree = 0;
+ if(IrNum >=12){
+ return 0;
+ }
+ IrF = IrData[0]/12;
+ IrS = IrData[0]%12;
+
+ if(IrF == 0 ){
+ if(IrS == 11){
+ irdegree = 15;
- }else{
- irrad = 345;
- }
-
- }
- irrad = irrad - (IrF - IrS)*15;
+ }else if(IrS == 1){
+ irdegree = 345;
+ }
+ return irdegree;
+ }
+ irdegree = 360 - IrF*30;
+ if(IrS&&(abs(IrF-IrS) == 1)){
+ irdegree = irdegree - (IrF - IrS)*15;
}
- return irrad;
+ if(irdegree>=360){
+ return 0;
+ }
+
+ return irdegree;
}
-void IrFrontAction( uint8_t IrMemo[],double ReV)//ball 12 or 0 o-clock
+void IrFrontAction()//ball 12 or 0 o-clock
{
+ if(IrData[1]>700){
+ if(abs(CompassPID)>10){
+ move(0,0,CompassPID,0);
+ return;
+ }
+ move(30,30,CompassPID,0);
+ return;
+ }
+ if(IrData[1]>600){
+ move(25,25,CompassPID,0);
+ return;
+ }
+ /*IrData[1]>500*/
+
+ if(PingData[0]<50){
+ if((PingData[1]<60)&&(PingData[1]>40)){
+ /*右側に居る*/
+ move(25,40,0,10);
+ wait_ms(100);
+ move(30,30,0,0);
+ return;
+ }
+ if((PingData[1]<60)&&(PingData[1]>40)){
+ /*左側に居る*/
+ move(40,25,0,-10);
+ wait_ms(100);
+ move(30,30,0,0);
+ return;
+ }
+ /*それ以外*/
+ move(10,10,CompassPID,0);
+ Receive();
+ if(!(IrNum == 0)) return;
+ move(40,40,CompassPID,0);
+ return ;
+ }
+ move(20,20,CompassPID,0);
+
+
}
-void IrBackAction( uint8_t IrMemo[],double ReV)//ball found six o-clock
+void IrBackAction()//ball found six o-clock
{
/***
* 6時にボールがある場合の処理．右と左のスペースを確認して広い方から回り込む
*
**/
- char Ping[2]={0};
-
- //PingReceiveRL(Ping);
-
-
- if(IrMemo[0] == 6){//check ir number
- return ;
- }
- if(Ping[0]<Ping[1]){
+ if(PingData[1]>PingData[3]){
/*右が大きい*/
-
-
+ if(IrData[1]>700){
+ move(-20,-20,CompassPID,45);
+ return;
+ }
+ if(IrData[1]>600){
+ move(-20,-20,CompassPID,60);
+ return;
+ }
+ move(-20,-20,CompassPID,90);
return;
}
/*左が大きい*/
-
-
+ if(IrData[1]>700){
+ move(-20,-20,CompassPID,-45);
+ return;
}
-
-void GoHome( uint8_t IrMemo[],double ReV)//Ball is not found.
-{
- //止まっとく
- S555.position(0.0);
+ if(IrData[1]>600){
+ move(-20,-20,CompassPID,-60);
+ return;
+ }
+ move(-20,-20,CompassPID,-90);
+ return;
- move(0,0,ReV,0);
-
- /*line検知をしないバージョン*/
- /*
- char Ping[2] = {0};
+}
+
+void GoHome()//Ball is not found.
+{
-
- PingReceiveFB(Ping);
- if(Ping[1] >=60){//後ろからの距離60cm
- move(-20,-20,ReV);
- PingReceiveFB(Ping);
+ if(PingData[2] >=60){//後ろからの距離60cm
+ move(-20,-20,CompassPID,0);
return ;
}
- move(0,0,ReV);//stop
- */
+ move(0,0,CompassPID,0);//stop
+
}
void PidUpdate()
{
-
- //Compass = ((hmc6352.sample() / 10) + 540 - CompassDef) % 360;
- wait_ms(10);
-
- //pid.setSetPoint((int)((REFERENCE + SetC) / 1.0));
- //InputPID = Compass;
- //pid.setProcessValue(InputPID);
- //CompassPID = -(pid.compute());
+ pid.setSetPoint((int)((REFERENCE + SetC) / 1.0));
+ InputPID = Compass;
+
+ pid.setProcessValue(InputPID);
+ CompassPID = -(pid.compute());
}
@@ -269,7 +391,6 @@
void tx_motor(){
array(speed[0],speed[1],speed[3],speed[2]);
Motor.printf("%s",StringFIN.c_str());
- S555.position(SetRad);
}
void SetUp(){
@@ -280,7 +401,7 @@
S555.calibrate(0.0005, 120.0);
- S555.position(0.0); //初期位置にセット
+ //S555.position(0.0); //初期位置にセット
move(0,0,0,0);//停止
Kick = 0;
@@ -292,6 +413,7 @@
pid.setBias(PID_BIAS); //pid sed def
pid.setMode(AUTO_MODE); //pid sed def
pid.setSetPoint(REFERENCE); //pid sed def
+ pidupdate.attach(&PidUpdate, PID_CYCLE);
@@ -300,11 +422,11 @@
void StartLoop(){
uint8_t State = 0;
- uint8_t Irnum = 0;
uint8_t LineData = 0;
while(1){
Led[0] = Led[1] = Led[2] = Led[3] = 1;
//Lcd.cls();
+
State = SwRead();
if(State == 0) continue;
@@ -327,8 +449,8 @@
}
if(State == Debug2){
while((State == Debug2)){
- //Irnum = IrReceiveFast();
- Lcd.printf("%d\n",Irnum);
+ Receive();
+ Lcd.printf("%d\n",IrNum);
wait_ms(100);
State = SwRead();
}
@@ -338,7 +460,7 @@
if(State == Debug3){
while((State == Debug3)){
- //Lcd.printf("%d\n",Compass);
+ Lcd.printf("%d\n",Compass);
wait_ms(100);
State = SwRead();
}
@@ -348,14 +470,14 @@
if(State == Kicker){
Led[0] = Led[1] = Led[2] = 0;
- /*move(20,20,0,0);
+ move(20,20,0,0);
while((State == Kicker)){
wait_ms(100);
State = SwRead();
}
move(0,0,0,0);
- */
+
continue;
}
}
@@ -363,101 +485,127 @@
}
int main() {
- /*Ir*/
- uint8_t IrNum = 12;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態
//uint8_t IrNumOld = 0;//過去値
/*Line*/
uint8_t LineData = 0;
-
+ uint8_t LinePing = 0;
- /*PID補正move加算値 Revise */
- double ReV = 0.0;
- double SetC;
-
+
/*State */
uint8_t LineIr = 0;
- uint8_t IrChange[13] ={0x01,0x01,0x02,0x02,0x02,0x04,
- 0x04,0x04,0x08,0x08,0x08,0x01,0x00};
+ uint8_t IrChange[13] ={0x01,0x01,0x03,0x02,0x02,0x06,
+ 0x04,0x04,0x0B,0x08,0x08,0x09,0x00};
+
/*行動設定*/
int Power = 0;
- int Rad = 0;
-
+ int Degree = 0;
/*楽しい変数達*/
- int nRad =0;//基礎角
- int addRad = 0;//追加角
+ int nDegree =0;//基礎角
+ int addDegree = 0;//追加角
/*関数ポインタ*/
- /*
- void (*AnotherAction[3])(uint8_t [],double);
+
+
+ //void (*AnotherAction[3])(uint8_t [],double);
+ void (*AnotherAction[3])();
AnotherAction[0] = IrFrontAction;
AnotherAction[1] = IrBackAction;
AnotherAction[2] = GoHome;
- */
+
SetUp();/*set up routine*/
Mbed.read_data(rx_data, ADDRESS);
Mbed.start_read();
//Mbed.check_rx_wait();
StartLoop(); /*loop strat, switch push end*/
+ Led[0] = Led[1] = Led[2] = Led[3] = 0;
+ wait_ms(100);
while(0){
- Power = 0;
- Led[0] = 1;
- Rad = 0;
- ReV = 0;
- SetC = 0.0;
+
- Receive();
-
+
+ Receive();
+ //Lcd.printf("%d\n",IrNum);
/*白線を読んでいないか確認する*/
LineData = (~Line+0x00) & 0x0F;
+
if(LineData){
- while(LineData){
- move(0,0,0,0);//
- Led[1] = Led[2] = Led[3] = 1;
- LineData = (~Line+0x00) & 0x0F;
- wait_ms(10);
+ LineIr = LineData & IrChange[IrNum];
+ LinePing = PingChange(LineData);
+ if(LineIr){
+ move(0,0,0,0);
+ while(LineIr){
+ Led[1] = Led[2] = Led[3] = 1;
+ Receive();
+ LineData = (~Line+0x00) & 0x0F;
+ LineIr = LineData & IrChange[IrNum];
+ wait_ms(10);
+ }
+ }else if(LinePing){
+ while(LinePing){
+ Led[1] = Led[2] = Led[3] = 1;
+ Receive();
+ LineData = (~Line+0x00) & 0x0F;
+ LinePing = PingChange(LineData);
+ wait_ms(10);
+ }
}
+
+
Led[1] = Led[2] = Led[3] = 0;
}
+ Power = 0;
+ Led[0] = 1;
+ Degree = 0;
+ SetC = 0.0;
+
Led[3] = 1;
+ Receive();
+ Degree = IrDegree();
- if(IrNum == 12){
- move(0,0,ReV,Rad);
+ if((Degree == 0)||(Degree == 180)||(IrNum == 12)){
+ (AnotherAction[IrNum/6])();
continue;
}
-
- Rad = IrRad();
- nRad = wrapRad[Rad/15];
+
+ /*
+ if(IrNum == 12){
+ move(0,0,0,0);
+ wait_ms(10);
+ continue;
+ }*/
+
+ nDegree = wrapDegree[Degree/15];
Power = 20;
-
-
- Rad = nRad + addRad;
- fool(&Rad,&Power);
- move(Power,Power,ReV,Rad);
+ Degree = nDegree + addDegree;
+ if((Degree <0)||(Degree>=360)){
+ Degree = 0;
+ }
+ fool(&Degree,&Power);
+ move(Power,Power,0,Degree);
+
+ //wait_ms(500);
+ Led[0] =0;
wait_ms(10);
- Led[0] =0;
-
-
}
-
+ int i;
while(1){
//デモプログラム
- Receive();
- wait(0.1);
- pc.printf("%d %d %d %d %d\n",IrData[0],IrData[1],IrData[2],PingData[0],PingData[1]);
- //pc.printf("%d %d %d %d %d\n",rx_data[0],rx_data[1],rx_data[2],rx_data[0],rx_data[1]);
-
+ Receive();
+ pc.printf("%d\t %d\t %d\t %d\t %d\t %d\t\n",rx_data[3],rx_data[4],rx_data[5],rx_data[6],rx_data[7],rx_data[8]);
+
+ wait(0.1);
}