CatPot 2015-2016
CatPot for defence on RoboCup in 2015 winter
Dependencies: AQM0802A HMC6352 MultiSerial PID Servo mbed
Diff: main.cpp
- Revision:
- 2:39135c67083d
- Parent:
- 1:e3248f278663
- Child:
- 3:2f74791564c9
--- a/main.cpp Tue Jan 27 14:03:48 2015 +0000
+++ b/main.cpp Wed Mar 11 01:11:02 2015 +0000
@@ -1,294 +1,92 @@
/***********************************
- *RoboCupJunior Soccer B Open 2014
- *Koshinestu progrum
+ *RoboCupJunior Soccer B Open 2015
+ *Koshinnestu progrum
*
- *このプログラムでは以下の処理をする．
- * LPC1114FN28/102からI2Cを用いて各種センサデータを収集
*
* データからロボットの移動やキッカー等のモータの動作を決定する処理を行う
*
* MotorDriverにmaxonに命令
*
- * ServoMotorでステアリング
+ * servoにステアリング指示
*
* LCDでデバック
*
* スイッチ4つとスタートスイッチで処理を実行
*
- * キッカー用のDigitalOutがどこかに入る
- *
+
*************************
- *
- *Pin Map
+ * Pin Map
+ *
+ * p5~p8 >> BusIn >> LineSensor
*
- * p5,p6,p7,p29,p30 >> SPI >>Stepping
- *
- * p9,p10 >> I2C orSerial >> LPC1114FN28/102 read
+ * p9,p10 >> I2C >> LPC1114FN28/102 read & Compass
*
- * p13,p14 >> Serial >> Motor
+ * p13,p14 >> Serial >> Motor
+ *
+ * p21 >> PwmOut >> Servo
*
- * p22~p26 >> DebugSw and StartSw
+ * p22~p26 >> DigitalIn >> DebugSw and StartSw
*
- * p27,p28 >> I2C >> DebugLCD
+ * p27,p28 >> I2C >> DebugLCD
*
- * p21 >> Kick
- *
- * p22 >> ServoMotor
- *
+ * p29 >> DigitalOut >> Kicker
+ *
+ * *never use pin number p11,p12,p15,p16,p17,p18,p19,p20,p30
+ *
+ *
******************************/
-
-/*
-
-****以下IRはNighは距離が近いものとする．
-
-*/
-
-
-
-
-
-
#include "mbed.h"
+#include <math.h>
+#include <sstream>
#include "PID.h"
#include "AQM0802A.h"
-#include "HMC6352.h"
+#include "MultiSerial.h"
#include "Servo.h"
-
-#include "def.h"
-
-#include <math.h>
-#include <sstream>
-
+#include "main.h"
-BusIn Sw(p22,p23,p24,p25,p26);
-DigitalOut Led[4] = {LED1,LED2,LED3,LED4};
-
-I2C Sensor(p9,p10);//sda,scl
-HMC6352 hmc6352(p9,p10);
-AQM0802A Lcd(p28,p27);//sda,scl
-
-Servo Servo(p21);
-
-Serial Motor(p13,p14);//tx,rx
-Serial pc(USBTX,USBRX);
-
-DigitalOut Kick(p22);
-
-extern string StringFIN;
-extern void array(int,int,int,int);
-
-int speed[4] = {0};
-
-/*Ir*/
-uint8_t IrNum;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態
-
-typedef enum {FRONT = 1, RIGHT, BACK, LEFT, NA} Direction;
-
-/*SensorData回収↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓*/
-unsigned int IrReceiveR(){
- /*順位解析版
- Slave側はIRを要求した場合IRのみを返してくるとする．
- irは値が大きいほうが近いと仮定する
-
-
- */
- char data_r[3],data_l[3];
- bool val;
- Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか
-
- val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信
- Led[0] = val;
- wait_ms(5);
- val = Sensor.read(ADDRESS_L|1, data_l, 3);
- Led[0] = !val;
+void Receive(void){
- if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/
- if((data_r[0] == 0)&&(data_l[0] == 0)){
- return 12;
- }
- if(data_r[0] == 0){
- return (data_l[0]/6+6)*12*12 + (data_l[0]%6+6)*12 + data_r[0]/6;
- }
- return data_r[0]/6*12*12 + (data_r[0]%6)*12 + data_l[0]/6+6;
-
- }
-
- if(data_r[2]>data_l[2]){
- if(data_r[2]>data_l[1]){
- return data_r[0]/6*12*12 + (data_r[0]%6)*12 + data_l[0]/6+6;
- }
-
- return data_r[0]/6*12*12 +(data_l[0]/6+6)*12+ data_r[0]%6;
-
- }else{
- if(data_l[2]>data_r[1]){
- return (data_l[0]/6+6)*12*12 + (data_l[0]%6+6)*12 + data_r[0]/6;
- }
-
- return (data_l[0]/6+6)*12*12 +(data_r[0]/6)*12+ (data_l[0]%6+6);
- }
-
-}
-
-uint8_t IrReceiveS(){
- /*単純解析版 一番大きい位置だけ返す
-
- Slave側はIRを要求した場合IRのみを返してくるとする．
- irは値が大きいほうが近いと仮定する
-
- */
- char data_r[3],data_l[3];
- bool val;
- Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか
-
- val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信
- Led[0] = val;
- wait_ms(5);
- val = Sensor.read(ADDRESS_L|1, data_l, 3);
- Led[0] = !val;
+ 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);
- if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/
- if((data_r[0] == 0)&&(data_l[0] == 0)){
- return 12;
- }
- if(data_r[0] == 0){
- return data_l[0]/6+6;
- }
- return data_r[0]/6;
- }
-
- if(data_r[2]>data_l[2]){
- return data_r[0]/6;
- }
- return data_l[0]/6+6;
-
-}
-uint8_t IrReceiveM(){
- /*値解析版 一番近い場所の値を返す（位置はとりあえずない）
- Slave側はIRを要求した場合IRのみを返してくるとする．
- irは値が大きいほうが近いと仮定する
-
-
+ /*
+ 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");
*/
- char data_r[3],data_l[3];
- bool val;
- Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか
-
- val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信
- Led[0] = val;
- wait_ms(5);
- val = Sensor.read(ADDRESS_L|1, data_l, 3);
- Led[0] = !val;
-
- if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/
- if((data_r[0] == 0)&&(data_l[0] == 0)){
- return 255;
- }
- if(data_r[0] == 0){
- return data_l[1];
- }
- return data_r[1];
- }
-
-
- if(data_r[2]>data_l[2]){
- return data_r[1];
-
- }
-
- return data_l[1];
-}
-unsigned int IrReceiveSM(){//16bit()
- /*値解析版 一番大きい値とその位置を返す
- Slave側はIRを要求した場合IRのみを返してくるとする．
- irは値が大きいほうが近いと仮定する
-
-
- */
- char data_r[3],data_l[3];
- bool val;
- Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか
-
- val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信
- Led[0] = val;
- wait_ms(5);
- val = Sensor.read(ADDRESS_L|1, data_l, 3);
- Led[0] = !val;
-
- if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/
- if((data_r[0] == 0)&&(data_l[0] == 0)){
- return (12<<8)+255;
- }
- if(data_r[0] == 0){
- return (data_l[0]/6+6)<<8 + data_l[1];
- }
- return data_r[0]/6<<8 + data_r[1];
+ if(IrData[0] == 255){
+ IrNum = 12;
+ return;
}
-
-
- if(data_r[2]>data_l[2]){
- return data_r[0]/6<<8 + data_r[1];
-
- }
- return ((data_l[0]/6)+6) <<8 + data_l[1];
-
-
-}
-uint8_t LineReceive(){
- //先に要求しない場合はLineの状況を送信すること．//4bit //前ー右ー後ろー左
- char data_r[2],data_l[2];
- bool val;
- //example val = slave.read(address,data,length,repeat);
- val = Sensor.read(ADDRESS_R|1, data_r, 1);
- Led[1] = val;
- wait_ms(5);
- val = Sensor.read(ADDRESS_L|1, data_l, 1);
- Led[1] = !val;
-
-
- return data_r[0]<<2 + data_l[0];
-
+ IrNum = IrData[0]/12;
}
-
-void PingReceiveRL(char ping[]){//ping[0]==Right,ping[1]==Left
- char ReadSelect[1] = {READ_PING};
- bool val;
- val = Sensor.write(ADDRESS_R|0, ReadSelect , 1);
- Led[2] = val;
- val = Sensor.read(ADDRESS_R|1, ping, 2);
- Led[2] = !val;
-
-
-}
-
-void PingReceiveFB(char ping[]){//ping[0]==FRONT,ping[1]==BACK
- char ReadSelect[1] = {READ_PING};
- bool val;
- val = Sensor.write(ADDRESS_L|0, ReadSelect , 1);
- Led[2] = val;
- val = Sensor.read(ADDRESS_L|1, ping, 2);
- Led[2] = !val;
-
-
-}
-
-/*SensorData回収↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑*/
-
-/*モーター駆動処理↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓*/
-
-void move(int vr, int vl){
+void move(int vr,int vl, double vs ,double Rad){
double pwm[4] = {0};
uint8_t i = 0;
- pwm[0] = -vr;
+ pwm[0] = vr - vs;
pwm[1] = 0;
pwm[2] = 0;
- pwm[3] = vl;
-
+ pwm[3] = vl*(-1.2) + vs;
+ pc.printf("%d\t %d\t %f\t %f\t\n", vr, vl, vs, Rad);
for(i = 0; i < 4; i++){
if(pwm[i] > 100){
pwm[i] = 100;
@@ -297,392 +95,184 @@
}
speed[i] = pwm[i];
}
+ SetRad = Rad;
+ wait_ms(10);
+}
+void fool (int *Rad, int *Power){
+ static int Last_Rad = 0;
+ static int Last_Vector = 1;
+ int rad = *Rad;
+ 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(Temp>160){
+ Last_Vector = -1 * Last_Vector;//正転逆転切り替え
+ if(*Rad/180){
+ *Rad = Angle[Last_Rad%180] -(Last_Rad - *Rad%180);
+ *Power = *Power * Last_Vector;
+ }else{
+ *Rad = Angle[Last_Rad%180] -(Last_Rad%180 - *Rad);
+ *Power = *Power * Last_Vector;
+ }
+ Last_Rad = rad;
+ return;
+ }else if((Last_Vector+2) == 1){
+ /*逆転のまま角度拡張*/
+ if(*Rad/180){
+ *Rad = -360 + *Rad ;
+ *Power = *Power * Last_Vector;
+ }else{
+ *Power = *Power * Last_Vector;
+ }
+ Last_Rad = rad;
+ 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;
+ }
+ Last_Rad = rad;
+ return;
+ }
+ }
+ }
+ /*通常動作*/
+ Last_Vector = RadToVector[(*Rad-1)/90];
+ *Rad = Angle[*Rad%180];
+ *Power = *Power * Last_Vector;
+ Last_Rad = rad;
+
+}
+int IrRad(){
+ /*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;
+
+ }else{
+ irrad = 345;
+ }
+
+ }
+ irrad = irrad - (IrF - IrS)*15;
+
+ }
+ return irrad;
}
-void ControlRobo0(int *CompassDef)//LeftFront 11時
+
+void IrFrontAction( uint8_t IrMemo[],double ReV)//ball 12 or 0 o-clock
{
}
-void ControlRobo1(int *CompassDef)//Front 12時
+
+void IrBackAction( uint8_t IrMemo[],double ReV)//ball found six o-clock
{
-/*前にボールがある場合の動作*/
- int Compass;
- char Ping[2];
- uint8_t Line,LineIr;
- unsigned int IrNumMax;
+ /***
+ * 6時にボールがある場合の処理．右と左のスペースを確認して広い方から回り込む
+ *
+ **/
+ char Ping[2]={0};
+
+ //PingReceiveRL(Ping);
+
- Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
- if(!((Compass>=150)&&(Compass<=210))){
- while(!((Compass>=150)&&(Compass<=210))){
- move(20,-20);//適当な回転。
- Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
- }
- return;
+ if(IrMemo[0] == 6){//check ir number
+ return ;
}
- if(IrReceiveM()<=150){//適当な150
- /*ステッピングを正面に設定*/
- /*busy_wait()*/
- /*モーターを前進*/
+ if(Ping[0]<Ping[1]){
+ /*右が大きい*/
+
+
return;
}
- PingReceiveRL(Ping);
- if((Ping[0]>90)&&(Ping[1]>40)){//数値は適当
- /*ステッピングを少しだけ傾ける*/
- /*モーターを左右差つけて回す(ロボットは少し傾く)*/
- /*busy_wait()*/
- /*モーターを前進*/
- while(1){
- Line = LineReceive();
-
- if(Line){ //ラインを検知していないか
- LineIr = Line & (IrReceiveS()/3 + 1);//計算により方位を合わせる。
- while(LineIr){
- move(0,0);//
- Led[1] = Led[2] = Led[3] = 1;
-
- LineIr = LineReceive() & (IrReceiveS()/3 + 1);
- }
- Led[1] = Led[2] = Led[3] = 0;
-
- Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
- while(!((Compass>=150)&&(Compass<=210))){
- move(20,-20);//適当な回転。
- Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
- }
- return;
- }
- Kick = 1;
- wait_ms(200);
- Kick = 0;
-
- IrNumMax = IrReceiveSM();//位置と大きさ
-
- if(!((IrNumMax&0x00)>>8) == 1){//この1はボールの関数の1を表す.
- Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
- while(!((Compass>=150)&&(Compass<=210))){
- move(20,-20);//適当な回転。
- Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
- }
- return;
- }
-
- if((IrNumMax&0x00FF)<150){
- Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
- while(!((Compass>=150)&&(Compass<=210))){
- move(20,-20);//適当な回転。
- Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
- }
- return;
- }
-
- }
-
- }else if((Ping[0]>40)&&(Ping[1]>90)){
-
- }else{
-
- }
-
-}
-void ControlRobo2(int *CompassDef)//RightFront 1時
-{
-
- int Compass;
- char Ir[2] ={0};//1, 1,2位, 1位の大きさ
- char ir_num,line;//わかりやすい名前
-
- Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
- if(!((Compass>=150)&&(Compass<=210))){
- while(!((Compass>=150)&&(Compass<=210))){
- move(20,-20);//適当な回転。
- Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
- }
- move(0,0);
- return;
- }
-
- /*irデータ取得 1,2位の場所、一位の距離*/
- if(!(Ir[0]%13 ==2)){
- return;
- }
-
-
- if(Ir[1]>=100){//近い場合
- //2位を含んだ計算
- /*motorset*/
- /*stepset*/
- return;
- }
-
- /*2位を含んだ計算*/
- /*計算された分stepmove*/
-
- move(20,20);
-
-
- /*line,ir get*/
- do{
- if(line){
- move(0,0);
- return;
- }
-
- /*line,ir get*/
- }while(ir_num == 2);
-
- move(0,0);
+ /*左が大きい*/
-}
-void ControlRobo3(int *CompassDef)//RightFront 2時
-{
- int Compass;
- char Ir[2] ={0};//1, 1,2位, 1位の大きさ
- char ir_num,line;//わかりやすい名前
-
- Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
- if(!((Compass>=150)&&(Compass<=210))){
- while(!((Compass>=150)&&(Compass<=210))){
- move(20,-20);//適当な回転。
- Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
- }
- move(0,0);
- return;
- }
-
- /*irデータ取得 1,2位の場所、一位の距離*/
- if(!(Ir[0]%13 ==3)){
- return;
- }
-
-
- if(Ir[1] >= 100){
- /*2位を含めた計算*/
- /*move()*/
- /*Stepset*/
- return;
- }
- /*2位を含めた計算*/
- /*Stepsetting 距離等により角度が変わる*/
-
- move(-20,-20);
- /*ir,line check*/
- do{
- if(line){
- move(0,0);
- return;
- }
- /*line,ir check*/
- }while(ir_num == 3);
-
- move(0,0);
-
-
-}
-void ControlRobo4(int *CompassDef)//Right 3時
-{
- int a = 50;
- unsigned int ir_sm;
- double value = 0;
- double v = 50;/* cm/s */
- double x = 20;/*モーターを回す割合,今は適当*/
-
- ir_sm = IrReceiveSM();
- if(ir_sm&0xF00>>8 == 4){
- return;
- }
- a = 300 - ir_sm&0x00FF ;//適当な計算//300は勘
- value = 10 * a * SHORT_LEN + 3*(a * a + SHORT_LEN * SHORT_LEN);
- value = PI *(3 * ( a + SHORT_LEN) - sqrt(value))/4/v;
-
- if(300 - ir_sm&0x00FF >200){
- //Step.Step(-10);//適当
-
- }
-
- /*SetPerm()
- *valueに応じた何かを計算する
- *ACCまたはMAX_SPEEDをいじればいい気がする
- *
- */
- //Step.Step(kakudo+10);
-
- /*x = v*何か*/
- move(-x,-x);
-
-
-}
-void ControlRobo5(int *CompassDef)//RightBack 4時
-{
- int Compass;
- char Ir[2] ={0};//1, 1,2位, 1位の大きさ
- char ir_num,line;//わかりやすい名前
-
- Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
- if(!((Compass>=150)&&(Compass<=210))){
- while(!((Compass>=150)&&(Compass<=210))){
- move(20,-20);//適当な回転。
- Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
- }
- move(0,0);
- return;
- }
- Servo.position(HOME);
-
- /*irデータ取得 1,2位の場所、一位の距離*/
- if(!(Ir[0]%13 == 5)){
- return;
}
-
- if(Ir[1] >=100){//近い
- /*計算*/
+void GoHome( uint8_t IrMemo[],double ReV)//Ball is not found.
+{
+ //止まっとく
+ S555.position(0.0);
+
+ move(0,0,ReV,0);
+
+ /*line検知をしないバージョン*/
+ /*
+ char Ping[2] = {0};
+
+
+ PingReceiveFB(Ping);
+ if(Ping[1] >=60){//後ろからの距離60cm
+ move(-20,-20,ReV);
+ PingReceiveFB(Ping);
+ return ;
+ }
- Servo.position(ONE);
- move(-20,-20);
- /*Step.setpaaa
- Step.move();-180+初期位置
- */
- /*ir,linecheck*/
- do{
- if(line){
- move(0,0);
- return;
- }
- if(!(ir_num >1 && ir_num <=5)){
- move(0,0);
- return;
- }
- /*ir,line check*/
- }while(1);
- }
- //遠い
-
- /*計算*/
-
- Servo.position(FOUR);
-
- move(-20,-20);
-
- /*ir,linecheck*/
- do{
- if(line){
- move(0,0);
- return;
- }
- if(!(ir_num >1 && ir_num <=5)){
- move(0,0);
- }
- /*ir,line check*/
- }while(1);
+ move(0,0,ReV);//stop
+ */
+}
-}
-void ControlRobo6(int *CompassDef)//BackRight 5時
-{
-
-
-}
-void ControlRobo7(int *CompassDef)//Back 6時
-{
- /*****
- * this function is drawing a semicircle.
- * semicircle manipulated ir_data&ping_data.
- *
- * 変数名は後から変えるためにわかりやすい名前にしておく
- **/
- uint8_t ir_num;
- uint8_t pingl,pingr;
+void PidUpdate()
+{
+ double inputPID = 0.0;
- /*
- ir(1位,2位,必要なら距離も)と超音波のデータを取得
- */
- if(ir_num==7){//一致しているかどうか念のため
- return ;
- }
- if(pingr<pingl){//本当ならば壁と垂直かどうか、確認すべき。コンパスや超音波の合計を見れば可能
- /*軌道の計算、楕円の半分見たいな感じの軌道がベスト*/
- /*半円なのであらかじめステッピングを回転させる必要がある*/
- /*モーター設定*/
- /*ステッピング設定*/
- return;
- }
- /*軌道の計算、楕円の半分見たいな感じの軌道がベスト*/
- /*半円なのであらかじめステッピングを回転させる必要がある*/
- /*モーター設定*/
- /*ステッピング設定*/
-
+ Receive();
- }
-void ControlRobo8(int *CompassDef)//BackLeft 7時
-{
- Servo.position(FOUR);
-}
-void ControlRobo9(int *CompassDef)//LeftBack 8時
-{
- Servo.position(FIVE);
-}
-void ControlRobo10(int *CompassDef)//Left 9時
-{
- Servo.position(SIX);
-}
-void ControlRobo11(int *CompassDef)//LeftFront 10時
-{
- Servo.position(NINE);
-}
-void GoHome(int *CompassDef)//Ball is not found.
-{
- /*line検知をしないバージョン*/
- int Compass;
- char ping[2] = {0};
+ pid.setSetPoint((int)((REFERENCE + SetAngle) / 1.0));
+ inputPID = ((Compass / 10) + 540 - CompassDef) % 360;;
- Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
- if(!((Compass>=150)&&(Compass<=210))){
- while(!((Compass>=150)&&(Compass<=210))){
- move(20,-20);//適当な回転。
- Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
- }
- /*return;してもいいかもしれない*/
- }
-
-
-
- PingReceiveFB(ping);
- if(ping[1] >=60){//後ろからの距離60cm
- while(ping[1] >=60){
- move(-20,-20);
- PingReceiveFB(ping);
- }
- }
-
- move(0,0);//stop
+ pid.setProcessValue(inputPID);
+ ReV = -(pid.compute());
}
-/*モーター駆動処理↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑*/
+
+
+
+
+
+
uint8_t SwRead(){
/******
- *返却値はスイッチの状況
- *参照元::aaaaa
- *
- *Calibration = 0x01;
- *Kicker = 0x02;
+ *retrun : sw_state
+ *StartS = 0x01;
+ *Debug2 = 0x02;
*Debug1 = 0x04;
- *Debug2 = 0x08;
- *StartS = 0x10;
+ *Debug3 = 0x06;
+ *Kicker = 0x08;
+ *Calibration = 0x10;
+
*
*****/
uint8_t i,temp,temp2;
- temp = Sw;
- if(!(temp == Calibration
- ||temp == Kicker
+ temp = ~Sw - 224;
+ if(!(temp == Kicker
||temp == Debug1
||temp == Debug2
- ||temp == StartS)) return 0;/*スイッチは押されていない状況*/
- if(temp !=0x00){
+ ||temp == Debug3
+ ||temp == StartS)) return 0;/*スイッチが押されていない*/
+ if(!(temp == 0x00)){
for(i = 0; i < 50; i++);
- temp2 = Sw;
+ temp2 = ~Sw - 224;
if(temp == temp2){
return temp;
}
@@ -692,265 +282,238 @@
//通信（モータ用）
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");
+ }*/
array(speed[0],speed[1],speed[3],speed[2]);
Motor.printf("%s",StringFIN.c_str());
+ S555.position(SetRad*11/9.0);
}
-void SetUp(int *compass_def){
+void SetUp(){
/*初期化*/
- Kick = 0;
- Sw.mode(PullUp);
Motor.baud(115200); //ボーレート設定
Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
- //move(0,0);//停止
+
- //Step.Resets();
- //Step.ReleseSW(0,1);
+ S555.calibrate(0.0005, 120.0);
+ S555.position(0.0); //初期位置にセット
+ move(0,0,0,0);//停止
- Servo.calibrate(0.0006, 120.0);
- Servo.position(HOME);//タイヤを前に向ける
+ Kick = 0;
+ Sw.mode(PullUp);
-/*
- hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20);
- *compass_def = (hmc6352.sample() / 10);
+ 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
- Lcd.cls();
- */
- /*初期化終了*/
-
+ for(int i=0; i<15; i++){
+ wait_ms(10);
+ Receive();
+ CompassDef = (Compass / 10);
+ PidUpdate();
+ }
}
void StartLoop(){
- /*
- *スイッチが押されるまでロボットはスタートしない．
- *
- *待っている間にほかのスイッチが押された場合は押されている間その動作をする等.
- *
- *switch割り当て
- *1.コンパスのキャリブレーション実行スイッチ
- *2.キッカーのキック(check用)
- *3,4.自由
- *5.StartSw
- */
+
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 == Calibration){
- /*calibration command enter...
- *
- *
- */
- continue;
+ if(State == StartS){
+ loop=0;
+ return;
+ /*loop end & start*/
}
- if(State == Kicker){
- /*
- *kicker check
- *
- *
- */
+
+ 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 == Debug1){
- /* debug command free
- *
- *display out to selected 3 menus. compass, ir, ping, line,etc...
- *
- **/
}
if(State == Debug2){
- /* debug command free
- *
- * decide movement of the beginning.
- *
- *
- **/
-
+ while((State == Debug2)){
+
+ Receive();
+
+ Lcd.printf("%d\n",IrNum);
+ wait_ms(100);
+ State = SwRead();
+ }
+ Lcd.cls();
+ continue;
}
- if(State == StartS){
- /*game start...*/
- return;
+
+ 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 == Kicker){
+ Led[0] = Led[1] = Led[2] = 0;
+ /*move(20,20,0,0);
+ while((State == Kicker)){
+ wait_ms(100);
+ State = SwRead();
+
+ }
+ move(0,0,0,0);
+ */
+ continue;
+ }
+ }
}
-
-void ClockPointing(int to_st, int power){
-
- /*
- to_st 時計において何時の方向にサーボを動かしたいか
- power タイヤのモータのパワー．絶対値．
-
- from_stで現在のサーボ状態保存．（初期がHOMEであること前提）
- これを利用して，３時⇔９時の移動のみサーボを動かさずにタイヤの駆動方向の反転のみで状態遷移可能．
-
- 改善の余地としてはワンショットタイマーを加えて
- 割り込み関数が作動するまでサーボ駆動時のタイヤの高出力を控え，
- 割り込み関数で引数の通りの出力でタイヤを駆動するというものがある．
- しかし場合によってはサーボの移動距離が短いときネックになることがある．
- 使うときはサーボの移動距離に応じて処理を変えるなどの工夫が必要になる．
- */
- static int from_st = TWELFTH;
- static int qb=0;
-
- int TireDir[] = {1, 1, 1, -1, -1, -1, -1, -1, 1, 1, 1, 1};//１時、２時、３時、４時…１２時//タイヤの駆動方向
- float ClockDir[]={ONE, TWO, THREE, FOUR, FIVE, SIX, SEVEN, EIGHT, NINE, TEN, ELEVEN, TWELVE};//サーボの方向
+void CompassFB(){
- //想定外の引数が来た時の対処
- if(
- (to_st > TWELFTH)
- || (to_st < FIRST)
-
- || (from_st > TWELFTH)
- || (from_st < FIRST)
-
- ) return;
-
-
- if(
- (from_st == THIRD || from_st == NINTH)
- && (to_st == THIRD || to_st == NINTH)
- && (from_st != to_st)
- ){
- qb = !qb;
- if(qb) for(int i=0; i<12; i++) TireDir[i] = TireDir[i]*(-1);//タイヤの駆動方向反転
+ for(int i=1; i<=18; i++){
+ if(abs(REFERENCE - Compass) <= i*10){
+ ReV = (i-1)*2;
+ if(Compass > REFERENCE){
+ ReV = ReV*(-1);
+ }
+ break;
+ }
}
- else{
- qb=0;
- Servo.position(ClockDir[to_st]);//サーボの方向転換
- }
-
- move(abs(power)*TireDir[to_st], abs(power)*TireDir[to_st]);//タイヤの出力
-
- from_st = to_st;//状態（サーボの）のプログラム上の遷移
-
}
int main() {
/*Ir*/
- //uint8_t IrNum;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態
- //→global変数
-
+ //→.h
/*Line*/
- uint8_t Line;
+ uint8_t LineData = 0;
- /*Compass*/
- int CompassDef = 0, Compass = 0;
+
+ /*PID補正move加算値 Revise */
+ //double ReV = 0.0;→global
+ //double SetC;
/*State */
- //Direction LineIr = NA;//方位設定奴...エラーでてめんどくさいので後でまたやることにする。
- uint8_t LineIr = NA;
+ //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 Rad = 0;//
+
+
+ /*楽しい変数達*/
+ int nRad =0;//基礎角
+ int addRad = 0;//追加角
/*関数ポインタ*/
- void (*ControlRobo[13])(int *);//irの位置による分岐
- ControlRobo[0] = ControlRobo0;
- ControlRobo[1] = ControlRobo1;
- ControlRobo[2] = ControlRobo2;
- ControlRobo[3] = ControlRobo3;
- ControlRobo[4] = ControlRobo4;
- ControlRobo[5] = ControlRobo5;
- ControlRobo[6] = ControlRobo6;
- ControlRobo[7] = ControlRobo7;
- ControlRobo[8] = ControlRobo8;
- ControlRobo[9] = ControlRobo9;
- ControlRobo[10] = ControlRobo10;
- ControlRobo[11] = ControlRobo11;
- ControlRobo[12] = GoHome;
+ /*
+ void (*AnotherAction[3])(uint8_t [],double);
+ AnotherAction[0] = IrFrontAction;
+ AnotherAction[1] = IrBackAction;
+ AnotherAction[2] = GoHome;
+ */
+
+ loop=1;
+ Mbed.read_data(rx_data, ADDRESS, DATA_NUM);
+ Mbed.start_read();
- SetUp(&CompassDef);/*set up routine*/
+ SetUp();/*set up routine*/
- //StartLoop(); /*loop stat, switch push end*/
+ StartLoop(); /*loop strat, switch push end*/
+
+ loop=0;
- /*
- 前を向くように設定をするべき
- アウトオブバウンズからの復帰時に反対を向けてスタートなので必要。
- */
- while(0){//前を向かせるwhile
- Compass = ((hmc6352.sample() / 10) + 540 - CompassDef) % 360;
- if(Compass == 3) break;//前を向いたら抜ける。
- }
- while(0) {
-
- Line = LineReceive();
-
- if(Line){ //ラインを検知していないか
- LineIr = Line & (IrReceiveS()/3 + 1);//計(lineとirの方位を合わせる。
- while(LineIr){
- move(0,0);//
+ //Monitor.attach(CompassFB, 0.1);
+
+ while(0){
+ Power = 0;
+ Led[0] = 1;
+ Rad = 0;
+ ReV = 0;
+ //SetC = 0.0;
+
+ Receive();
+
+ /*白線を読んでいないか確認する*/
+ LineData = (~Line+0x00) & 0x0F;
+ if(LineData){
+ while(LineData){
+ move(0,0,0,0);//
Led[1] = Led[2] = Led[3] = 1;
-
- LineIr = LineReceive() & (IrReceiveS()/3 + 1);
+ LineData = (~Line+0x00) & 0x0F;
+ wait_ms(10);
}
Led[1] = Led[2] = Led[3] = 0;
- continue;
-
+
}
- IrNum = IrReceiveS();
+ Led[3] = 1;
- (*ControlRobo[IrNum])(&CompassDef);//irの位置によったループ等の処理に移る。
+ /*if(IrNum == 12){
+ move(0,0,ReV,Rad);
+ continue;
+ }*/
+
+ //Rad = IrRad();
+ //nRad = wrapRad[Rad/15];
+ Power = 20;
+
+
+ //Rad = nRad + addRad;
- //wait_ms(0);
+ //fool(&Rad,&Power);
+ move(Power,Power,ReV,Rad);
+
+ wait_ms(10);
+
+ Led[0] =0;
+
+
}
- move(0,0);
- Servo.position(HOME);
- uint8_t clock[]={TWELFTH, THIRD, SIXTH, NINTH};
- uint8_t clock2[]={FIRST, SECOND, THIRD, FOURTH, FIFTH, SIXTH, SEVENTH, EIGHTH, NINTH, TENTH, ELEVENTH, TWELFTH};
- Servo.position(HOME);
while(1){
-
- /*デモプログラム
- *
- *正方形の軌道
- *
- *円軌道
- *
- *反復横跳び
- *
- *関数ClockPointingを活用することで少しだけ楽に以上の動きを実現できる．
- */
- for(int i=0; i < 4; i++){
-
- ClockPointing(clock[i], 20);
-
- wait(2);
-
- }
-
- for(int i=0; i < 12; i++){
-
- ClockPointing(clock2[i], 20);
-
- wait(1);
-
- }
-
- for(int i=0; i < 4; i++){
-
- ClockPointing(THIRD, 20);
-
- wait(2);
-
- ClockPointing(NINTH, 20);
-
- wait(2);
- }
-
+ //デモプログラム
+ Receive();
+ wait_ms(50);
}
+
+
+
}