CatPot 2015-2016
CatPot for defence on RoboCup in 2015 winter
Dependencies: AQM0802A HMC6352 MultiSerial PID Servo mbed
Diff: main.cpp
- Revision:
- 3:2f74791564c9
- Parent:
- 2:39135c67083d
- Child:
- 4:7c488c059498
--- a/main.cpp Wed Mar 11 01:11:02 2015 +0000
+++ b/main.cpp Wed Mar 11 07:47:19 2015 +0000
@@ -39,54 +39,83 @@
#include "mbed.h"
#include <math.h>
#include <sstream>
+
+#include "Servo.h"
#include "PID.h"
#include "AQM0802A.h"
-#include "MultiSerial.h"
-#include "Servo.h"
#include "main.h"
-void Receive(void){
-
- IrData[0] = rx_data[0];
- IrData[1] = rx_data[1];
- IrData[2] = rx_data[2];
+//#include "MultiSerial.h"
+/*
+void Receive(){
+ IrData[0] = rx_data[0];
+ IrData[1] = rx_data[1];
+ IrData[2] = rx_data[2];
PingData[0] = rx_data[3];
PingData[1] = rx_data[4];
PingData[2] = rx_data[5];
PingData[3] = rx_data[6];
- Compass =(rx_data[7]<<0) |
- (rx_data[8]<<8);
+
- /*
- pc.printf("%d\t", IrData[0]);
- pc.printf("%d\t", IrData[1]);
- pc.printf("%d\t", IrData[2]);
- pc.printf("%d\t", PingData[0]);
- pc.printf("%d\t", PingData[1]);
- pc.printf("%d\t", PingData[2]);
- pc.printf("%d\t", PingData[3]);
- pc.printf("%d\t", Compass);
- //pc.printf("%d\t", Mbed.__readSize);
- pc.printf("\n");
- */
-
- if(IrData[0] == 255){
+ if((IrData[0] == 255)||(IrData[1] == 255)||(IrData[2] == 255)){
IrNum = 12;
return;
}
IrNum = IrData[0]/12;
+
}
-
-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;
- pc.printf("%d\t %d\t %f\t %f\t\n", vr, vl, vs, Rad);
+ double vias=10.0;
+ double dif;
+
+ dif=(((Compass / 10) + 540 - CompassDef) % 360) - REFERENCE;
+ if(abs(dif)<vias){
+
+ pwm[0] = vr;
+ pwm[1] = 0;
+ pwm[2] = 0;
+ pwm[3] = vl*(1.0)*(-1.0);
+ }
+ else{
+
+ if(dif<0){
+ //vs = vs - (20-vias);
+ vs = -(20+(abs(dif))*0.1);
+ if( abs(dif) >= 100) vs = -(20+(100)*0.1);
+ }
+ else{
+ //vs = vs + (20-vias);
+ vs = (20+(abs(dif))*0.1);
+ if( abs(dif) >= 100) vs = (20+(100)*0.1);
+ }
+
+ pwm[0] = vs;
+ pwm[1] = 0;
+ pwm[2] = 0;
+ pwm[3] = vs*(-1)*(-1);
+ }
+ if(Loop==1){
+ pwm[0] = 0;
+ pwm[1] = 0;
+ pwm[2] = 0;
+ pwm[3] = 0;
+ }
+ /*if(vs<0){
+ pwm[0] = vr - vs*k;
+ pwm[1] = 0;
+ pwm[2] = 0;
+ pwm[3] = vl*1.2 + vs*k;
+ }else{
+ pwm[0] = vr + vs*k;
+ pwm[1] = 0;
+ pwm[2] = 0;
+ pwm[3] = (vl - vs*k)*1.2;
+ }*/
for(i = 0; i < 4; i++){
if(pwm[i] > 100){
pwm[i] = 100;
@@ -95,154 +124,275 @@
}
speed[i] = pwm[i];
}
- SetRad = Rad;
+ if(Degree > 110){
+ Degree = 110;
+ }else if(Degree < -110){
+ Degree = -110;
+ }
+ SetDegree = Degree;
+ S555.position(SetDegree*11.0/9.0);
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]>70){
+ if(abs(CompassPID)>10){
+ move(0,0,CompassPID,0);
+ return;
+ }
+ move(30,30,CompassPID,0);
+ return;
+ }
+ if(IrData[1]>60){
+ 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]>70){
+ move(-20,-20,CompassPID,45);
+ return;
+ }
+ if(IrData[1]>60){
+ move(-20,-20,CompassPID,60);
+ return;
+ }
+ move(-20,-20,CompassPID,80);
return;
}
/*左が大きい*/
-
-
+ if(IrData[1]>70){
+ move(-20,-20,CompassPID,-45);
+ return;
}
-
-void GoHome( uint8_t IrMemo[],double ReV)//Ball is not found.
-{
- //止まっとく
- S555.position(0.0);
+ if(IrData[1]>60){
+ move(-20,-20,CompassPID,-60);
+ return;
+ }
+ move(-20,-20,CompassPID,-80);
+ 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()
{
- double inputPID = 0.0;
- Receive();
-
- pid.setSetPoint((int)((REFERENCE + SetAngle) / 1.0));
- inputPID = ((Compass / 10) + 540 - CompassDef) % 360;;
-
- pid.setProcessValue(inputPID);
- ReV = -(pid.compute());
-
+ pid.setSetPoint((int)((REFERENCE + SetC) / 1.0));
+ InputPID = ((Compass/10) + 540 - CompassDef) % 360;
+
+ pid.setProcessValue(InputPID);
+ CompassPID = (pid.compute());
}
@@ -269,6 +419,8 @@
||temp == Debug1
||temp == Debug2
||temp == Debug3
+ ||temp == Debug12
+ ||temp == Debug23
||temp == StartS)) return 0;/*スイッチが押されていない*/
if(!(temp == 0x00)){
for(i = 0; i < 50; i++);
@@ -283,29 +435,28 @@
//通信（モータ用）
void tx_motor(){
- /*if(loop==3){
- speed[0]=0;
- speed[1]=0;
- speed[2]=0;
- speed[3]=0;
- Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
- }else{
- Motor.printf("1F0202F0003F0204F000\r\n");
- }*/
+ /*
+ speed[0]=0;
+ speed[1]=0;
+ speed[2]=0;
+ speed[3]=0;
+ */
array(speed[0],speed[1],speed[3],speed[2]);
Motor.printf("%s",StringFIN.c_str());
- S555.position(SetRad*11/9.0);
}
void SetUp(){
/*初期化*/
+
+
Motor.baud(115200); //ボーレート設定
Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
-
- S555.calibrate(0.0005, 120.0);
- S555.position(0.0); //初期位置にセット
+ Mbed.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み（センサ用）
+
+ S555.calibrate(0.0006, 120.0);
+ //S555.position(0.0); //初期位置にセット
move(0,0,0,0);//停止
Kick = 0;
@@ -315,31 +466,30 @@
pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); //pid sed def
pid.setBias(PID_BIAS); //pid sed def
pid.setMode(AUTO_MODE); //pid sed def
- pid.setSetPoint(REFERENCE); //pid sed def
+ pid.setSetPoint(REFERENCE); //pid sed def
for(int i=0; i<15; i++){
- wait_ms(10);
- Receive();
- CompassDef = (Compass / 10);
- PidUpdate();
+ CompassDef = (Compass / 10);
+ wait_ms(10);
}
+ pidupdate.attach(&PidUpdate, PID_CYCLE);
+
}
void StartLoop(){
uint8_t State = 0;
uint8_t LineData = 0;
-
while(1){
Led[0] = Led[1] = Led[2] = Led[3] = 1;
//Lcd.cls();
+
State = SwRead();
if(State == 0) continue;
if(State == StartS){
- loop=0;
- return;
/*loop end & start*/
+ return;
}
if(State == Debug1){
@@ -356,10 +506,9 @@
}
if(State == Debug2){
while((State == Debug2)){
-
- Receive();
-
+ //Receive();
Lcd.printf("%d\n",IrNum);
+ //pc.printf("%d\t %d\t %d\t %d\t %d\t %d\t\n",IrData[0],IrData[1],IrData[2],PingData[0],PingData[1],PingData[2]);
wait_ms(100);
State = SwRead();
}
@@ -369,11 +518,27 @@
if(State == Debug3){
while((State == Debug3)){
-
- Receive();
-
Lcd.printf("%d\n",((Compass / 10) + 540 - CompassDef) % 360);
-
+ wait_ms(100);
+ State = SwRead();
+ }
+ Lcd.cls();
+ continue;
+ }
+ if(State == Debug12){
+ while((State == Debug12)){
+ Lcd.printf("%d\n", PingData[0]);//Left
+ Lcd.printf("%d\n", PingData[1]);//Right
+ wait_ms(100);
+ State = SwRead();
+ }
+ Lcd.cls();
+ continue;
+ }
+ if(State == Debug23){
+ while((State == Debug23)){
+ Lcd.printf("%d\n", PingData[2]);//Back
+ Lcd.printf("%d\n", PingData[3]);//Front
wait_ms(100);
State = SwRead();
}
@@ -382,135 +547,155 @@
}
if(State == Kicker){
- Led[0] = Led[1] = Led[2] = 0;
- /*move(20,20,0,0);
+ while((State == Kicker)){
+ Lcd.printf("%f\n",CompassPID);
+ wait_ms(100);
+ State = SwRead();
+ }
+ Lcd.cls();
+ continue;
+
+ /*Led[0] = Led[1] = Led[2] = 0;
+ Kick = 1;
+ wait_ms(500);
+ Kick = 0;
while((State == Kicker)){
wait_ms(100);
State = SwRead();
-
}
- move(0,0,0,0);
- */
- continue;
+ continue;*/
}
- }
+ }
}
-
-void CompassFB(){
-
- for(int i=1; i<=18; i++){
- if(abs(REFERENCE - Compass) <= i*10){
- ReV = (i-1)*2;
- if(Compass > REFERENCE){
- ReV = ReV*(-1);
- }
- break;
- }
- }
-}
-
int main() {
- /*Ir*/
- //→.h
+ //uint8_t IrNumOld = 0;//過去値
/*Line*/
uint8_t LineData = 0;
-
+ uint8_t LinePing = 0;
+
- /*PID補正move加算値 Revise */
- //double ReV = 0.0;→global
- //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 LineIr = 0;
+ 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;
- */
- loop=1;
-
- Mbed.read_data(rx_data, ADDRESS, DATA_NUM);
- Mbed.start_read();
-
+
SetUp();/*set up routine*/
-
+
StartLoop(); /*loop strat, switch push end*/
+ Led[0] = Led[1] = Led[2] = Led[3] = 0;
+ wait_ms(100);
- loop=0;
-
- //Monitor.attach(CompassFB, 0.1);
+ Loop = 0;
- while(0){
- Power = 0;
- Led[0] = 1;
- Rad = 0;
- ReV = 0;
- //SetC = 0.0;
+ while(1){
- Receive();
-
+ S555.calibrate(0.0005, 120.0);
+
+ //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){
+ move(0,0,0,0);
+ 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;
+ }
+
+ /*
+ if(IrNum == 12){
+ move(0,0,0,0);
+ wait_ms(10);
+ continue;
}*/
-
- //Rad = IrRad();
- //nRad = wrapRad[Rad/15];
+
+ 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,CompassPID,Degree);
+ //wait_ms(500);
+ Led[0] =0;
wait_ms(10);
- Led[0] =0;
-
-
}
-
- while(1){
+ while(0){
+ //デモプログラム
+ //Receive();
+ pc.printf("%d %d %d %d %d\n",IrData[0],IrData[1],IrData[2],PingData[0],PingData[1]);
+ //pc.printf("%d %d %d %d\n",PingData[1],PingData[2],PingData[3],Compass);
- //デモプログラム
- Receive();
- wait_ms(50);
+ //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]);
+ //pc.printf("%d\t %d\t %d\t %d\n",speed[0],speed[1],speed[2],speed[3]);
+ wait(0.1);
}