CatPot 2015-2016
version 3 通信方式，マイコン等に変更あり
Dependencies: AQM0802A PID Servo mbed
Diff: main.cpp
- Revision:
- 0:65b9e62cc2b6
- Child:
- 1:f91d53098d57
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Mar 06 01:16:05 2015 +0000
@@ -0,0 +1,463 @@
+/***********************************
+ *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 "PID.h"
+#include "AQM0802A.h"
+#include "MultiSerial.h"
+#include "Servo.h"
+#include "main.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]+rx_data[8];
+
+ if(IrData[0] == 255){
+ IrNum = 12;
+ return;
+ }
+ IrNum = IrData[0]/12;
+
+
+}
+
+void move(int vr,int vl, double vs ,double Rad){
+ double pwm[4] = {0};
+ uint8_t i = 0;
+ pwm[0] = vr - vs;
+ pwm[1] = 0;
+ pwm[2] = 0;
+ pwm[3] = vl*1.2 + vs;
+
+ 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];
+ }
+ 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 IrFrontAction( uint8_t IrMemo[],double ReV)//ball 12 or 0 o-clock
+{
+
+}
+
+void IrBackAction( uint8_t IrMemo[],double ReV)//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]){
+ /*右が大きい*/
+
+
+ return;
+ }
+ /*左が大きい*/
+
+
+
+ }
+
+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 ;
+ }
+
+ move(0,0,ReV);//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());
+}
+
+
+
+
+
+
+
+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 == 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());
+ S555.position(SetRad);
+}
+
+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); //初期位置にセット
+ 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
+
+ Mbed.read_data(rx_data, ADDRESS);
+ Mbed.start_read();
+
+
+}
+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;
+
+ 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)){
+ //Irnum = IrReceiveFast();
+ Lcd.printf("%d\n",Irnum);
+ wait_ms(100);
+ State = SwRead();
+ }
+ Lcd.cls();
+ continue;
+ }
+
+ if(State == Debug3){
+ while((State == Debug3)){
+ //Lcd.printf("%d\n",Compass);
+ 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;
+ }
+ }
+
+}
+int main() {
+
+ /*Ir*/
+ uint8_t IrNum = 12;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態
+ //uint8_t IrNumOld = 0;//過去値
+ /*Line*/
+ uint8_t LineData = 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};
+ /*行動設定*/
+ int Power = 0;
+ int Rad = 0;
+
+
+ /*楽しい変数達*/
+ int nRad =0;//基礎角
+ int addRad = 0;//追加角
+
+
+
+ /*関数ポインタ*/
+ /*
+ void (*AnotherAction[3])(uint8_t [],double);
+ AnotherAction[0] = IrFrontAction;
+ AnotherAction[1] = IrBackAction;
+ AnotherAction[2] = GoHome;
+ */
+ SetUp();/*set up routine*/
+ StartLoop(); /*loop strat, switch push end*/
+
+ while(1){
+ 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;
+ LineData = (~Line+0x00) & 0x0F;
+ wait_ms(10);
+ }
+ Led[1] = Led[2] = Led[3] = 0;
+
+ }
+
+
+ Led[3] = 1;
+
+ if(IrNum == 12){
+ move(0,0,ReV,Rad);
+ continue;
+ }
+
+ Rad = IrRad();
+ nRad = wrapRad[Rad/15];
+ Power = 20;
+
+
+ Rad = nRad + addRad;
+
+ fool(&Rad,&Power);
+ move(Power,Power,ReV,Rad);
+
+ wait_ms(10);
+
+ Led[0] =0;
+
+
+ }
+
+ /*
+ while(0){
+ //デモプログラム
+ S555.position(0.0);
+
+ wait(1.5);
+ S555.position(90.0);
+
+ }
+
+ */
+
+
+}