CatPot 2015-2016
version 3 通信方式，マイコン等に変更あり
Dependencies: AQM0802A PID Servo mbed
main.cpp
- Committer:
- ryuna
- Date:
- 2015-03-13
- Revision:
- 6:bfc0e8d2d3ae
- Parent:
- 5:09afcbe0c18f
File content as of revision 6:bfc0e8d2d3ae:
/***********************************
*RoboCupJunior Soccer B Open 2015
*Koshinnestu progrum
*
*
* データからロボットの移動やキッカー等のモータの動作を決定する処理を行う
*
* MotorDriverにmaxonに命令
*
* servoにステアリング指示
*
* LCDでデバック
*
* スイッチ4つとスタートスイッチで処理を実行
*
*************************
* Pin Map
*
* p5~p8 >> BusIn >> LineSensor
*
* p9,p10 >> I2C >> LPC1114FN28/102 read & Compass
*
* p13,p14 >> Serial >> Motor
*
* p21 >> PwmOut >> Servo
*
* p22~p26 >> DigitalIn >> DebugSw and StartSw
*
* p27,p28 >> I2C >> DebugLCD
*
* p29 >> DigitalOut >> Kicker
*
* *never use pin number p11,p12,p15,p16,p17,p18,p19,p20,p30
*
*
******************************/
#include "mbed.h"
#include <math.h>
#include <sstream>
#include "Servo.h"
#include "PID.h"
#include "AQM0802A.h"
#include "main.h"
void ServoWait(){
WaitFlag = 0;
}
void GoHinit(){
GoHFlag = 0;
}
void move(int vr,int vl, double vs ,int Degree){
double pwm[4] = {0};
uint8_t i = 0;
static int last_deg;
double vias=30.0;
double dif;
if(abs(last_deg)<23){
dif=(((Compass / 10) + 540 - CompassDef) % 360) - REFERENCE;
if(abs(dif)<vias){
pwm[0] = vr;
pwm[1] = 0;
pwm[2] = 0;
pwm[3] = vl;
}
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;
}
}else{
pwm[0] = vr;
pwm[1] = 0;
pwm[2] = 0;
pwm[3] = vl;
}
if(!WaitFlag){
SetDegree = Degree * 11/9;
if(SetDegree > 110){
SetDegree = 110;
}else if(SetDegree < -110){
SetDegree = -110;
}
S555.position(SetDegree);
WaitFlag = 1;
STimer.attach(&ServoWait,WAIT_TIME);
last_deg = Degree;
}
for(i = 0; i < 4; i++){
if(pwm[i] > 100){
pwm[i] = 100;
}else if(pwm[i] < -100){
pwm[i] = -100;
}
speed[i] = pwm[i];
}
wait_ms(10);
}
uint8_t PingChange(uint8_t LineData){
static uint8_t Last_Line;
static uint8_t Last_Ping;
uint8_t LinePing = 0;
uint8_t ReData =0;
if(!LineData){
return 0;
}
if(PingData[0] <50) LinePing = LinePing + 1;
if(PingData[1] <50) LinePing = LinePing + 2;
if(PingData[2] <50) LinePing = LinePing + 4;
if(PingData[3] <50) LinePing = LinePing + 8;
if(LinePing&0x01){
if((LineData&0x01) ||(Last_Line&0x01)||(Last_Ping&0x01)){
ReData += 1;
}
}
if(LinePing&0x02){
if((LineData&0x02) ||(Last_Line&0x02)||(Last_Ping&0x02)){
ReData += 2;
}
}
if(LinePing&0x04){
if((LineData&0x04) ||(Last_Line&0x04)||(Last_Ping&0x04)){
ReData += 4;
}
}
if(LinePing&0x08){
if((LineData&0x08) ||(Last_Line&0x08)||(Last_Ping&0x08)){
ReData += 8;
}
}
if(ReData){
Last_Ping = LinePing;
Last_Line = LineData;
return ReData;
}
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 degree = *Degree;
int Temp;
if((*Degree <0)||(*Degree >=360)){
*Degree = 0;
Last_Degree = 0;
Last_Vector = 1;
return ;
}
Temp = Last_Degree % 180;
if((Temp>70) &&(Temp<110)){
Temp = *Degree % 180;
if((Temp>70) &&(Temp<110)){
Temp = abs(*Degree - Last_Degree);
if(Temp>160){
Last_Vector = -1 * Last_Vector;//正転逆転切り替え
if(*Degree/180){
*Degree = Angle[Last_Degree%180] -(Last_Degree - *Degree%180);
*Power = *Power * Last_Vector;
}else{
*Degree = Angle[Last_Degree%180] -(Last_Degree%180 - *Degree);
*Power = *Power * Last_Vector;
}
Last_Degree = degree;
if((*Degree <= -120)||(*Degree >=120)){
*Degree = 0;
}
return;
}else if((Last_Vector+2) == 1){
/*逆転のまま角度拡張*/
if(*Degree/180){
*Degree = -360 + *Degree ;
}
*Power = *Power * Last_Vector;
Last_Degree = degree;
if((*Degree <= -120)||(*Degree >=120)){
*Degree = 0;
}
return;
}else if((Last_Vector+2) == 3){
/*正転のまま*/
if(*Degree/180){
*Degree = -360 + *Degree ;
}
*Power = *Power * Last_Vector;
Last_Degree = degree;
if((*Degree <= -120)||(*Degree >=120)){
*Degree = 0;
}
return;
}
}
}
/*通常動作*/
if(*Degree == 0){
Last_Vector = DegreeToVector[0];
*Degree = Angle[*Degree%180];
*Power = *Power * Last_Vector;
Last_Degree = degree;
return ;
}
if(*Degree == 270){
Last_Vector = DegreeToVector[(*Degree)/90];
*Degree = -90;
*Power = *Power * Last_Vector;
Last_Degree = degree;
return;
}
Last_Vector = DegreeToVector[(*Degree-1)/90];
*Degree = Angle[*Degree%180];
*Power = *Power * Last_Vector;
Last_Degree = degree;
if((*Degree <= -120)||(*Degree >=120)){
*Degree = 0;
}
}
int IrDegree(){
/*irの1位の値，2位の場所からirの360へ持っていく*/
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 if(IrS == 1){
irdegree = 345;
}
return irdegree;
}
irdegree = 360 - IrF*30;
if(IrS&&(abs(IrF-IrS) == 1)){
irdegree = irdegree - (IrF - IrS)*15;
}
if(irdegree>=360){
return 0;
}
return irdegree;
}
void IrFrontAction()//ball 12 or 0 o-clock
{
if(IrData[1]>65){
move(30,30,CompassPID,0);
return;
}
if(IrData[1]>57){
move(20,20,CompassPID,0);
return;
}
/*IrData[1]>500*/
move(10,10,0,0);
if(PingData[0]<60){
if((PingData[1]<60)&&(PingData[1]>40)){
/*右側に居る*/
if(IrData[1]<50){
move(40,25,0,10);
Kick = 1;
wait_ms(200);
move(30,30,0,0);
Kick = 0;
return;
}
move(40,25,0,10);
wait_ms(200);
move(30,30,0,0);
return;
}
if((PingData[1]<60)&&(PingData[1]>40)){
/*左側に居る*/
if(IrData[1]<50){
move(25,40,0,-10);
Kick = 1;
wait_ms(200);
move(30,30,0,0);
Kick = 0;
return;
}
move(25,40,0,-10);
wait_ms(200);
move(30,30,0,0);
return;
}
/*それ以外*/
//Receive();
if(!(IrNum == 0)) return;
move(40,40,CompassPID,0);
Kick = 1;
wait_ms(100);
Kick = 0;
return ;
}
move(20,20,CompassPID,0);
}
void IrBackAction()//ball found six o-clock
{
/***
* 6時にボールがある場合の処理．右と左のスペースを確認して広い方から回り込む
*
**/
int Pingrl = PingData[1] + PingData[3];
if(Pingrl >=120){
if(PingData[1]>PingData[3]){
/*右が大きい*/
if(IrData[1]>65){
move(-25,-25,CompassPID,45);
return;
}
if(IrData[1]>55){
move(-20,-20,CompassPID,60);
return;
}
move(-20,-20,CompassPID,90);
return;
}
/*左が大きい*/
if(IrData[1]>65){
move(-25,-25,CompassPID,-45);
return;
}
if(IrData[1]>55){
move(-20,-20,CompassPID,-60);
return;
}
move(-20,-20,CompassPID,-90);
return;
}
if(PingData[1]>PingData[3]){
/*右が大きい*/
if(IrData[1]>65){
move(-25,-25,0,30);
return;
}
if(IrData[1]>55){
move(-20,-20,0,60);
return;
}
move(-15,-15,0,70);
return;
}
/*左が大きい*/
if(IrData[1]>65){
move(-25,-25,CompassPID,-30);
return;
}
if(IrData[1]>55){
move(-20,-20,CompassPID,-60);
return;
}
move(-15,-15,CompassPID,-70);
return;
}
void GoHome()//Ball is not found.
{
int pingRl =0;
int PowLev;
if(!GoHFlag){
GoHFlag = 1;
GoHTimer.attach(&GoHinit,1.0);
move(0,0,0,0);
wait_ms(100);
}
if(PingData[2] >=50){//後ろからの距離60cm
move(-25,-25,0,0);
return ;
}
if(PingData[2] <= 25){
move(15,15,0,0);
return ;
}
pingRl = PingData[1]+PingData[3];
if(pingRl <120){
move(0,0,0,0);//stop
return;
}
if(PingData[1] < 70){
PowLev = (70 - PingData[1]);
move(PowLev,PowLev,0,90);
return ;
}else if (PingData[3] < 70){
PowLev = (70 - PingData[3]);
move(PowLev,PowLev,0,-90);
return ;
}
move(0,0,0,0);//stop
}
void PidUpdate()
{
pid.setSetPoint((int)((REFERENCE + SetC) / 1.0));
InputPID = ((Compass/10) + 540 - CompassDef) % 360;
pid.setProcessValue(InputPID);
CompassPID = (pid.compute());
/*
pid.setSetPoint((int)((REFERENCE + SetC) / 1.0));
InputPID = Compass;
pid.setProcessValue(InputPID);
CompassPID = (pid.compute());
*/
}
uint8_t SwRead(){
/******
*retrun : sw_state
*StartS = 0x01;
*Debug2 = 0x02;
*Debug1 = 0x04;
*Debug3 = 0x06;
*Kicker = 0x08;
*Calibration = 0x10;
*
*****/
uint8_t i,temp,temp2;
temp = ~Sw - 224;
if(!(temp == Kicker
||temp == Debug1
||temp == Debug2
||temp == Debug3
||temp == Debug12
||temp == Debug23
||temp == StartS)) return 0;/*スイッチが押されていない*/
if(!(temp == 0x00)){
for(i = 0; i < 50; i++);
temp2 = ~Sw - 224;
if(temp == temp2){
return temp;
}
}
return 0;
}
//通信（モータ用）
void tx_motor(){
array(speed[0],speed[1],speed[3],speed[2]);
Motor.printf("%s",StringFIN.c_str());
}
void SetUp(){
/*初期化*/
Motor.baud(115200); //ボーレート設定
Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
Mbed.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み（センサ用）
S555.calibrate(0.0006, 120.0);
//S555.position(0.0); //初期位置にセット
move(0,0,0,0);//停止
Kick = 0;
Sw.mode(PullUp);
pid.setInputLimits(MINIMUM,MAXIMUM); //pid sed def
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
for(int i=0; i<15; i++){
CompassDef = Compass/10;
wait_ms(10);
}
//pidupdate.attach(&PidUpdate, PID_CYCLE);
}
void StartLoop(){
uint8_t State = 0;
uint8_t LineData = 0;
int a;
while(1){
Led[0] = Led[1] = Led[2] = Led[3] = 1;
//Lcd.cls();
State = SwRead();
if(State == 0) continue;
if(State == StartS){
/*loop end & start*/
return;
}
if(State == Debug1){
while((State == Debug1)){
LineData = (~Line+0x00) & 0x0F;
Lcd.printf("%d\n",LineData);
wait_ms(100);
State = SwRead();
}
Lcd.cls();
continue;
}
if(State == Debug2){
while((State == Debug2)){
//Receive();
Lcd.printf("%d\t%d\n",IrNum,IrData[1]);
wait_ms(100);
State = SwRead();
}
Lcd.cls();
continue;
}
if(State == Debug3){
while((State == Debug3)){
a = (Compass/10 + 540 - CompassDef) % 360;
Lcd.printf("%d\n",a);
wait_ms(100);
State = SwRead();
}
Lcd.cls();
continue;
}
if(State == Debug12){
while((State == Debug12)){
Lcd.printf("%d %d\n%d %d\n",PingData[0],PingData[1],PingData[2],PingData[3]);
wait_ms(100);
State = SwRead();
}
Lcd.cls();
continue;
}
if(State == Kicker){
Led[0] = Led[1] = Led[2] = 0;
Kick = 1;
wait_ms(500);
Kick = 0;
while((State == Kicker)){
wait_ms(100);
State = SwRead();
}
continue;
}
}
}
int main() {
//uint8_t IrNumOld = 0;//過去値
/*Line*/
uint8_t LineData = 0;
uint8_t LinePing = 0;
/*State */
uint8_t LineIr = 0;
uint8_t IrChange[13] ={0x01,0x01,0x02,0x02,0x02,0x04,
0x04,0x04,0x08,0x08,0x08,0x01,0x00};
/*行動設定*/
int Power = 0;
int Degree = 0;
/*楽しい変数達*/
int nDegree =0;//基礎角
int addDegree = 0;//追加角
int Temp = 0;
int StopCt = 0;
/*関数ポインタ*/
//void (*AnotherAction[3])(uint8_t [],double);
void (*AnotherAction[3])();
AnotherAction[0] = IrFrontAction;
AnotherAction[1] = IrBackAction;
AnotherAction[2] = GoHome;
SetUp();/*set up routine*/
StartLoop(); /*loop strat, switch push end*/
Led[0] = Led[1] = Led[2] = Led[3] = 0;
wait_ms(100);
while(1){
//S555.calibrate(0.0006, 120.0);
//Receive();
//Lcd.printf("%d\n",IrNum);
/*白線を読んでいないか確認する*/
LineData = (~Line+0x00) & 0x0F;
if(LineData){
LineIr = LineData & IrChange[IrNum];
LinePing = PingChange(LineData);
if(LineIr){
move(0,0,0,0);
StopCt = 0;
while(LineIr){
if(StopCt>30){
GoHome();
wait_ms(100);
}
StopCt++;
Led[1] = Led[2] = Led[3] = 1;
LineData = (~Line+0x00) & 0x0F;
LineIr = LineData & IrChange[IrNum];
wait_ms(10);
}
}else if(LinePing){
move(0,0,0,0);
StopCt = 0;
while(LinePing){
if(StopCt>30){
GoHome();
wait_ms(300);
}
StopCt++;
Led[1] = Led[2] = Led[3] = 1;
if((LinePing&0x06)||(LinePing&0x0C)){
if(LinePing&0x06){
move(15,15,0,45);
}else if(LinePing&0x0C){
move(15,15,0,-45);
}
wait_ms(200);
}
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;
Degree = IrDegree();
if((Degree == 0)||(Degree == 180)||(IrNum == 12)){
(AnotherAction[IrNum/6])();
continue;
}
/*
if(IrNum == 12){
move(0,0,0,0);
wait_ms(10);
continue;
}*/
nDegree = wrapDegree[Degree/15];
Power = 35;
if((Degree>30)&&(Degree<75)){
if(IrData[1] < 56){
if(Degree>180){/*へらす*/
addDegree = -10;
}else{
addDegree = 10;
}
}
}
Temp = Degree%180;
if((Degree==75)){
if(IrData[1]<55){
}
}
if((Temp>80)&&(Temp<100)){
if(PingData[2]<40){
if(Degree>180){
addDegree = 100;
}else{
addDegree = -100;
}
}else{
if(IrData[1] > 65){
if(Degree>180){
addDegree = 40;
}else{
addDegree = -40;
}
}else {
addDegree = 0;
}
}
}
/*if(IrData[1] > 65){
if(Degree>180){
addDegree = 50;
}else{
addDegree = -50;
}
}else if(IrData[1] >55){
if(Degree>180){
addDegree = 30;
}else{
addDegree = -30;
}
}else {
addDegree = 0;
}*/
if(PingData[2]<40){
if((Degree>=150)&&(Degree<=300)){
if(Degree>180){
addDegree = 90;
}else{
addDegree = -90;
}
}
}else if(PingData[2]<70){
if((Degree>=150)&&(Degree<=300)){
if(Degree>180){
addDegree = 40;
}else{
addDegree = -40;
}
}
}
if(((Degree>=0)&&(Degree<=30))||((Degree>=330)&&(Degree<360))){
if(IrData[1]<55){
Power = 15;
}
}
if(((Degree>30)&&(Degree<75))||((Degree>315)&&(Degree<330))){
if(IrData[1]<55){
Power = 32;
}
}
Degree = nDegree + addDegree;
if((Degree <0)||(Degree>=360)){
Degree = 0;
}
//Lcd.printf("%d\n",Degree);
fool(&Degree,&Power);
move(Power,Power,0,Degree);
//wait_ms(500);
//Lcd.cls();
Led[0] =0;
wait_ms(10);
}
int i;
while(1){
//デモプログラム
//Receive();
for(i = 0; i<360; i++){
Power = 20;
fool(&i,&Power);
move(Power,Power,0,i);
wait_ms(200);
}
//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);
//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);
}
}