Naoya Muramatsu
ロボカップJrのブロック大会2015で使用したプログラム
Dependencies: BurstSPI LSM9DS0 PID PWMMotorDriver SDFileSystem mbed
Diff: main.cpp
- Revision:
- 0:ab144c574081
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed May 11 09:39:07 2016 +0000
@@ -0,0 +1,1278 @@
+/*------------------------
+
+* XBee1
+ ID : 5102
+
+ DH : 0013A200
+ DL : 407ABE67
+
+* XBee2
+ ID : 5102
+
+ DH : 0013A200
+ DL : 407C04BD
+
+------------------------*/
+
+/*------------------------
+mv /Users/muramatsunaoya/Downloads/RoboCup_2015_LPC1768.bin /Volumes/MBED/ && diskutil unmount /Volumes/MBED/
+------------------------*/
+
+
+
+#include "mbed.h"
+// ライブラリ
+#include "MotorDriver.h"
+#include "PID.h"
+// 自作のクラス
+#include "xbeeAPI.h"
+
+
+/*--- 定義 ---*/
+#define PI 3.1415926
+#define Deg2Rad (3.1415926/180)
+#define Byte2Rad (3.1415926/255/2)
+
+#define mC 261.626
+#define mD 293.665
+#define mE 329.628
+#define mF 349.228
+#define mG 391.995
+#define mA 440.000
+#define mB 493.883
+
+// PIDに関する部分
+#define IndividualValueL 0.4
+#define IndividualValueR 0.4
+#define IndividualValueB 0.4
+
+// 0.4 300.0 0.00001
+#define PID_Kc 0.6
+#define PID_Ti 0.0 // <- 0.0でもいいかも？
+#define PID_Td 0.03
+#define RATE 0.00006
+
+#define PID_Kp 0.2
+#define PID_Ki 0.0
+#define PID_Kd 0.0
+
+
+
+
+// csピン <-> mbedピン の対応
+ #define cs01 p23
+ #define cs02 p22
+ #define cs03 p15
+ #define cs04 p14
+ #define cs05 p16
+ #define cs06 p17
+ #define cs07 p18
+ #define cs08 p19
+ #define cs09 p20
+ #define cs10 p30
+ #define cs11 p29
+ #define cs12 p26
+ #define cs13 p25
+
+// デバッグ用の定義_
+ // #define PING_RLB_ // 超音波（左,右,後）
+ #define MOTOR_ // モータドライバ
+ // #define KICK_DIV_ // キッカー, ドリブラー
+ // #define IR_FR_ // IRボール距離（右前）
+ // #define IR_BR_ // IRボール距離（右後）
+ // #define IR_BL_ // IRボール距離（左前）
+ // #define IR_FL_ // IRボール距離（左後）
+ #define IR__ // IRボール方向
+ // #define SDCARD_ // SDカード
+ #define PING_F_ // 超音波（前,斜め）
+ #define IMU_ // IMU（LSM9DS0）
+ #define DEBUG_ // デバッグ
+// #define MESSAGE
+// #define PROG_TIME
+
+// SPI通信に関するパラメータ
+#define SPI_WAIT_US 2 // SPI通信の待ち時間(us)
+#define t_SS_SCLK 0.002 // SSを指定してから、通信開始までの待ち時間(us)
+
+// IRに関するマスク用数値
+ #define DEG045 0x80
+ #define DEG090 0x40
+ #define DEG135 0x20
+ #define DEG180 0x10
+ #define DEG225 0x08
+ #define DEG270 0x04
+ #define DEG315 0x02
+ #define DEG000 0x01
+
+
+
+/*--- ピンの設定 ---*/
+// LEDの設定
+DigitalOut led_Motor(LED1, 0);
+DigitalOut led_IR(LED2, 0);
+DigitalOut led_IMU(LED3, 0);
+DigitalOut led4(LED4, 0);
+// SW
+DigitalIn SW(p24); // OFF then 1, ON then 0
+// ラインセンサの許可信号
+DigitalOut linePermission(p5, 1);
+// ラインセンサのinputピン
+DigitalIn lineLeft(p6);
+DigitalIn lineRight(p7);
+// Kickerのピン設定
+DigitalIn kickerInput(cs03);
+DigitalOut kickerOuput(p8, 1);
+// SPI通信用のピン設定
+ SPI spi(p11, p12, p13); // mosi, miso, clk
+ DigitalOut cs_PingRLB(cs01, 1); // 超音波（左,右,後）
+ DigitalOut cs_Motor(cs02, 1); // モータドライバ
+ // DigitalOut cs_KickerDribbler(cs03, 1); // キッカー, ドリブラー
+ DigitalOut cs_Speed(cs04, 1); // マウスセンサ（ADNS9800）
+ DigitalOut cs_IRFrDistance(cs05, 1); // IRボール距離（右前）
+ DigitalOut cs_IRBrDistance(cs06, 1); // IRボール距離（右後）
+ DigitalOut cs_IRBlDistance(cs07, 1); // IRボール距離（左前）
+ DigitalOut cs_IRFlDistance(cs08, 1); // IRボール距離（左後）
+ DigitalOut cs_IR(cs09, 1); // IRボール方向
+ DigitalOut cs_SDcard(cs10, 1); // SDカード
+ DigitalOut cs_PingF(cs11, 1); // 超音波（前,斜め）
+ DigitalOut cs_IMU(cs12, 1); // IMU（LSM9DS0）
+ DigitalOut cs_Debug(cs13, 1); // デバッグ
+Ticker pidupdata;
+// デバッグ用のSerial通信用のピン設定
+PwmOut sound(p21);
+#if 0
+Serial pc(p9, p10); // tx, rx
+#else
+Serial pc(USBTX, USBRX); // tx, rx
+#endif
+
+// #define XBEE
+#ifdef XBEE
+Serial xbee(p9, p10);
+#endif
+
+
+
+/*--- グローバル変数 ---*/
+int LoopCount = 0;
+PID pidController(PID_Kc, PID_Ti, PID_Td, RATE); // Kp, Ki, Kd, RATE
+float moveDirection = 0.0; // ロボットの進む方向
+// PINGに関する変数
+uint8_t pingFF, pingFR, pingFL;
+uint8_t pingBR, pingBL, pingR, pingL;
+// ロボットの行動に関する変数
+int Integral=0;
+int Direction = 0;
+int8_t Angle=0, ex_Angle=0;
+int dx=0, ex_dx=0, ex2_dx=0;
+float R = 2.3;
+float Omega = 0.0;
+float Theta = 0.0;
+
+// デバッグに関する変数
+Timer t;
+uint8_t MyRemoteAddress[] = {0x00,0x13,0xa2,0x00,0x40,0x7c,0x04,0xbd}; // コントローラのアドレス
+
+
+// プロトタイプ宣言
+ inline int bitCount(uint8_t bits);
+ inline void lineMove();
+ inline void lineCheck();
+ inline bool SWcheck();
+ void PingRead();
+ inline int IRRead();
+ void ImuRead();
+ void PwmLRB1(float* pPwmL, float* pPwmR, float* pPwmB, float vx, float vy, float speed);
+ void PwmLRB2(float* pPwmL, float* pPwmR, float* pPwmB, float vx, float vy, float speed);
+ void PwmLRB3(float* pPwmL, float* pPwmR, float* pPwmB, float vx, float vy, float speed);
+ void RegulationHeading(float* pPwmL, float* pPwmR, float* pPwmB);
+ inline void Motor(float direction, float speed);
+ inline float MoveDirection(int iBallDirection);
+ bool spiChecking();
+ inline void ControlCommand(uint8_t command);
+ inline void serialChecking();
+
+
+
+
+/*--- メッセージ表示用関数 ---*/
+void msgD(int iTab, char *cMsg, int iValue, bool bNew=true)
+{
+ #ifdef MESSAGE
+ for (int i=0; i<iTab; i++) pc.printf(" ");
+ pc.printf("%s : %d", cMsg, iValue);
+ if (bNew) pc.printf("\n");
+ else pc.printf(" ");
+ return;
+ #endif
+}
+void msgF(int iTab, char *cMsg, float fValue, bool bNew=true)
+{
+ #ifdef MESSAGE
+ for (int i=0; i<iTab; i++) pc.printf(" ");
+ pc.printf("%s : %7.4f", cMsg, fValue);
+ if (bNew) pc.printf("\n");
+ else pc.printf(" ");
+ return;
+ #endif
+}
+void msgX(int iTab, char *cMsg, uint8_t ucValue, bool bNew=true)
+{
+ #ifdef MESSAGE
+ for (int i=0; i<iTab; i++) pc.printf(" ");
+ pc.printf("%s : %02x", cMsg, ucValue);
+ if (bNew) pc.printf("\n");
+ else pc.printf(" ");
+ return;
+ #endif
+}
+/*--- エラー表示用関数 ---*/
+void errorMsgHex(int iTab, char *cMsg, uint8_t ucValue, bool bNew=true) // tab, message, value, new line
+{
+ #ifdef MESSAGE
+ for (int i=0; i<iTab; i++) pc.printf(" ");
+ pc.printf("error: %s rx_data: %02x", cMsg, ucValue);
+ if (bNew) pc.printf("\n");
+ else pc.printf(" ");
+ return;
+ #endif
+}
+/*--- BitCount関数 ---*/
+inline int bitCount(uint8_t bits)
+{
+ bits = (bits & 0x55) + (bits >> 1 & 0x55);
+ bits = (bits & 0x33) + (bits >> 2 & 0x33);
+ bits = (bits & 0x0f) + (bits >> 4 & 0x0f);
+ bits = (bits & 0xff) + (bits >> 8 & 0xff);
+ return (bits & 0xff) + (bits >>16 & 0xff);
+}
+
+inline void lineMove()
+{
+#if 1
+ /*--- 反対方向に進む ---*/
+ // if (Direction == 1000.0)
+ // return;
+ // else if (Direction > 0.0)
+ // Motor(Direction-180.0, 1.0);
+ // else
+ // Motor(Direction+180.0, 1.0);
+ // wait(0.4);
+ int stopDirecetion = 0;
+ if (Direction > 0.0)
+ stopDirecetion = Direction-180.0;
+ else
+ stopDirecetion = Direction+180.0;
+ for (int i=0; i<700; i++) {
+ ImuRead();
+ pidController.setProcessValue(Angle);
+ Omega = pidController.compute();
+ Motor(1000.0, 0.0);
+ }
+ Motor(stopDirecetion, 0.4);
+ wait(0.3);
+#endif
+
+#if 0
+ /*--- 中央に戻ろうとする ---*/
+ if (Direction == 1000.0)
+ return;
+ else if (lineLeft == 1 && lineRight == 0) {
+ Motor(-90.0, 0.5);
+ }
+ else if (lineLeft == 0 && lineRight == 1) {
+ Motor(90.0, 0.5);
+ }
+ else if (lineLeft == 1 && lineRight == 1) {
+ if (Direction > 0.0)
+ Motor(Direction-180.0, 1.0);
+ else
+ Motor(Direction+180.0, 1.0);
+ }
+ else return;
+#endif
+
+#if 0
+ /*--- IRsensorの状況を見て行動 ---*/
+ while (IRRead() != 1000.0) {
+ ImuRead();
+ pidController.setProcessValue(Angle);
+ Omega = pidController.compute();
+ Motor(moveDirection, 0.5);
+ Direction = moveDirection;
+ }
+#endif
+}
+
+
+inline void lineCheck()
+{
+ // ラインセンサの状態をチェック
+ if (lineLeft || lineRight) {
+ lineMove();
+ }
+}
+
+inline bool SWcheck()
+{
+ /*---
+ ボタンが押されているかをチェック
+ - ON-> '1'
+ - OFF-> '0'
+ ---*/
+ if (!SW) {
+ wait(0.3);
+ if (SW) {
+ wait(0.2);
+ return 1;
+ }
+ }
+ return 0;
+}
+
+/*--- 超音波による距離の測定 ---*/
+void PingRead()
+{
+ cs_PingF = 0;
+ pingFF = spi.write(0x01); // 開始信号
+ cs_PingF = 1;
+ cs_PingF = 0;
+ pingFF = spi.write(0x02); // 正面
+ cs_PingF = 1;
+ cs_PingF = 0;
+ pingFR = spi.write(0x03); // 右前
+ cs_PingF = 1;
+ cs_PingF = 0;
+ pingFL = spi.write(0x04); // 左前
+ cs_PingF = 1;
+
+ #if 0
+ cs_PingRLB = 0;
+ pingL = spi.write(0x01); // 開始信号
+ cs_PingRLB = 1;
+ cs_PingRLB = 0;
+ pingL = spi.write(0x02); // 左
+ cs_PingRLB = 1;
+ cs_PingRLB = 0;
+ pingR = spi.write(0x03); // 右
+ cs_PingRLB = 1;
+ cs_PingRLB = 0;
+ pingBL = spi.write(0x04); // 左後ろ
+ cs_PingRLB = 1;
+ cs_PingRLB = 0;
+ pingBR = spi.write(0x05); // 右後ろ
+ cs_PingRLB = 1;
+ #endif
+}
+
+/*--- ロボットから見たボールの方向を返す ---*/
+inline int IRRead()
+{
+ /*---
+
+ - IRのnearとfarのデータを取得
+ - ボールのある方向を返す
+
+ ---*/
+
+ uint8_t near, far;
+ // データを取得する
+ cs_IR = 0; wait_us(t_SS_SCLK); far = spi.write(0x00); cs_IR = 1;
+ wait_us(SPI_WAIT_US);
+ cs_IR = 0; wait_us(t_SS_SCLK); near = ~spi.write(0xFF); cs_IR = 1;
+ wait_us(SPI_WAIT_US);
+ // データを解析して、進行方向を決定
+ int iDirection = 0;
+ int directionNear = 0;
+ int directionFar = 0;
+ // int bitCountNear = bitCount(near);
+ // int bitCountFar = bitCount(far);
+
+ // directionNear = ( ((DEG045 & near)?1:0) * 45
+ // + ((DEG090 & near)?1:0) * 90
+ // + ((DEG135 & near)?1:0) * 135
+ // + ((DEG180 & near)?1:0) * 180
+ // + ((DEG225 & near)?1:0) * 225
+ // + ((DEG270 & near)?1:0) * 270
+ // + ((DEG315 & near)?1:0) * 315
+ // + ((DEG000 & near)?1:0) * 000)
+ // / bitCountNear;
+ // directionFar = ( ((DEG045 & far)?1:0) * 45
+ // + ((DEG090 & far)?1:0) * 90
+ // + ((DEG135 & far)?1:0) * 135
+ // + ((DEG180 & far)?1:0) * 180
+ // + ((DEG225 & far)?1:0) * 225
+ // + ((DEG270 & far)?1:0) * 270
+ // + ((DEG315 & far)?1:0) * 315
+ // + ((DEG000 & far)?1:0) * 000)
+ // / bitCountFar;
+
+
+ int iBitCountNear = 0;
+ bool bBitFlag = false;
+ int aiAngleData[] = {0, -45, -90, -135, 180, 135, 90, 45};
+ int iAngleSum = 0;
+ int iBaseAngle = 0;
+ for (int i=0, j=0; i<8; i++) {
+ if (near & 0x01) {
+ iBitCountNear++;
+ if (bBitFlag) {
+ iAngleSum += aiAngleData[i-j];
+ }
+ else {
+ bBitFlag = true;
+ iBaseAngle = aiAngleData[i];
+ j = i;
+ }
+ }
+ near >>= 1;
+ }
+ iAngleSum /= iBitCountNear;
+ directionNear = iBaseAngle + iAngleSum;
+
+ if (iBitCountNear > 0) {
+ iDirection = directionNear;
+ }
+ // else if (bitCountFar > 0) {
+ // iDirection = directionFar;
+ // }
+ else {
+ iDirection = 1000.0;
+ }
+
+ // 角度を -180~+180 に修正
+ if (iDirection > 180.0 && iDirection != 1000.0){
+ iDirection -= 360.0;
+ }
+ else if (iDirection < -180.0 && iDirection != 1000.0){
+ iDirection += 360.0;
+ }
+
+ msgD(0, "Direction", iDirection);
+
+ return iDirection;
+}
+
+/*--- Union ---*/
+union uni_8bits {
+ uint8_t uint8;
+ int8_t int8;
+};
+/*--- IMUのデータを取得、Angleに代入 ---*/
+void ImuRead()
+{
+ // 変数宣言
+ int count = 5;
+ uni_8bits data = {0};
+ // データを受信
+ do {
+ cs_IMU = 0;
+ data.uint8 = spi.write(0x50);
+ cs_IMU = 1;
+ count--;
+ wait_us(SPI_WAIT_US);
+ } while ((data.uint8==0xFF || data.uint8==0x00) && count);
+ // 0~255 -> -128~127
+ Angle = data.int8;
+ msgD(0, "IMU", Angle);
+}
+/*--- 偏差から操作量Omegaを求める関数 ---*/
+#if 0
+void GetOmega() {
+ // 偏差の差を求める
+ if (-3<=Angle && Angle<=3) Angle=0;
+
+ dx = Angle - ex_Angle;
+
+ if (-3<=dx && dx<=3) {
+ dx = 0;
+ Angle = ex_Angle = (Angle+ex_Angle) / 2;
+ }
+ Integral /= 10;
+ Integral += (Angle + ex_Angle) / 2;
+
+ #if 1
+ // 操作量Omegaを求める（PID制御）
+ Omega = - (PID_Kp * (float)(Angle / 127.0))
+ - (PID_Ki * (float)(Integral / 127.0))
+ - (PID_Kd * (float)(dx / 127.0));
+ #else
+ // webページものも
+ Omega+= PID_Kp * (float)(dx)
+ +PID_Ki * (float)(Angle)
+ +PID_Kd * (float)((dx - ex_dx));
+ #endif
+
+ if (Omega < -1.0) {
+ Omega = -1.0;
+ }
+ else if (Omega > 1.0) {
+ Omega = 1.0;
+ }
+
+ msgF(0, "Omega", Omega);
+
+ // データをずらす
+ ex_dx = dx;
+ ex_Angle = Angle;
+}
+#endif
+
+/*--- pwmLRBを求める関数 ---*/
+void PwmLRB1(float* pPwmL, float* pPwmR, float* pPwmB, float vx, float vy, float speed)
+{
+ /*---
+
+ このサイトに載っている公式を使用 (http://wiki.tokor.org/index.php?%A5%AA%A5%E0%A5%CB%A5%DB%A5%A4%A1%BC%A5%EB%A4%CB%A4%E8%A4%EB%C1%B4%CA%FD%B8%FE%B0%DC%C6%B0%BC%D6%A4%CE%C0%A9%B8%E6%A1%A1-%A1%A1%B5%A1%B3%A3%C0%A9%B8%E6)
+
+ ---*/
+ float pwmL = vx * cos(Theta+PI*5/6)+ vy * sin(Theta+PI*5/6) - R*Omega;
+ float pwmR = vx * cos(Theta+PI/6) + vy * sin(Theta+PI/6) - R*Omega;
+ float pwmB = vx * cos(Theta-PI/2) + vy * sin(Theta-PI/2) - R*Omega;
+ /*--- 最大値を求める ---*/
+ float pwmMAX = fabs(pwmL);
+ float aPwmR = fabs(pwmR);
+ float aPwmB = fabs(pwmB);
+ pwmMAX = (pwmMAX > aPwmR) ? pwmMAX : aPwmR;
+ pwmMAX = (pwmMAX > aPwmB) ? pwmMAX : aPwmB;
+ /*--- 求めた最大値で正規化 ---*/
+ pwmL = pwmL / pwmMAX * speed * IndividualValueL;
+ pwmR = pwmR / pwmMAX * speed * IndividualValueR;
+ pwmB = pwmB / pwmMAX * speed * IndividualValueB;
+
+ /*--- 値を反映 ---*/
+ *pPwmL = pwmL;
+ *pPwmR = pwmR;
+ *pPwmB = pwmB;
+}
+void PwmLRB2(float* pPwmL, float* pPwmR, float* pPwmB, float vx, float vy, float speed)
+{
+ float pwmL = vx * cos(Theta+PI*5/6)+ vy * sin(Theta+PI*5/6);
+ float pwmR = vx * cos(Theta+PI/6) + vy * sin(Theta+PI/6) ;
+ float pwmB = vx * cos(Theta-PI/2) + vy * sin(Theta-PI/2) ;
+ /*--- 最大値を求める ---*/
+ float pwmMAX = fabs(pwmL);
+ float aPwmR = fabs(pwmR);
+ float aPwmB = fabs(pwmB);
+ pwmMAX = (pwmMAX > aPwmR) ? pwmMAX : aPwmR;
+ pwmMAX = (pwmMAX > aPwmB) ? pwmMAX : aPwmB;
+ /*--- 求めた最大値で正規化 ---*/
+ pwmL = pwmL / pwmMAX * speed * IndividualValueL * 0.7 - 0.3*Omega;
+ pwmR = pwmR / pwmMAX * speed * IndividualValueR * 0.7 - 0.3*Omega;
+ pwmB = pwmB / pwmMAX * speed * IndividualValueB * 0.7 - 0.3*Omega;
+
+ /*--- 値を反映 ---*/
+ *pPwmL = pwmL;
+ *pPwmR = pwmR;
+ *pPwmB = pwmB;
+}
+void PwmLRB3(float* pPwmL, float* pPwmR, float* pPwmB, float vx, float vy, float speed)
+{
+ /*---
+
+ このサイトに載っていた方法の応用を使う (http://yukispanicworld.tumblr.com/post/106053191409)
+ - 旋回は、求めた値の割合の変化分を足し合わせることによって行う
+
+ ---*/
+ float pwmL = vx * cos(Theta+PI*5/6)+ vy * sin(Theta+PI*5/6);
+ float pwmR = vx * cos(Theta+PI/6) + vy * sin(Theta+PI/6) ;
+ float pwmB = vx * cos(Theta-PI/2) + vy * sin(Theta-PI/2) ;
+ /*--- 最大値を求める ---*/
+ float pwmMAX = fabs(pwmL);
+ float aPwmR = fabs(pwmR);
+ float aPwmB = fabs(pwmB);
+ pwmMAX = (pwmMAX > aPwmR) ? pwmMAX : aPwmR;
+ pwmMAX = (pwmMAX > aPwmB) ? pwmMAX : aPwmB;
+ /*--- 求めた最大値で正規化 ---*/
+ pwmL = pwmL / pwmMAX * speed;
+ pwmR = pwmR / pwmMAX * speed;
+ pwmB = pwmB / pwmMAX * speed;
+ /*--- 旋回を考慮する ---*/
+ float k = 0.07;
+ pwmL += pwmL * k*Omega;
+ pwmR += pwmR * k*Omega;
+ pwmB += pwmB * k*Omega;
+ /*--- 値を反映 ---*/
+ *pPwmL = pwmL;
+ *pPwmR = pwmR;
+ *pPwmB = pwmB;
+}
+void RegulationHeading(float* pPwmL, float* pPwmR, float* pPwmB)
+{
+ float k = 1.0; // 調節用の係数
+ float pwmL = - Omega * k * IndividualValueL;
+ float pwmR = - Omega * k * IndividualValueR;
+ float pwmB = - Omega * k * IndividualValueB;
+
+ /*--- 値を反映 ---*/
+ *pPwmL = pwmL;
+ *pPwmR = pwmR;
+ *pPwmB = pwmB;
+}
+/*--- モータの計算と通信 ---*/
+inline void Motor(float direction, float speed)
+{
+ uint8_t rx_data;
+
+ /*--- 計算に必要となる値 ---
+ * 方向 ... direction (deg)
+ * 速度 ... speed (m/s)
+ * 機体の回転数 ... Omega (rad/s)
+ * 機体の半径 ... R (m)
+ * 機体の基準に対する傾き ... Theta (rad)
+ * pwmL(左), pwmB(後), pwmR(右)
+ --*/
+
+ // 変数宣言
+ float pwmL, pwmB, pwmR;
+
+
+
+ // 停止コマンドのときの処理
+ if (direction == 1000.0) {
+ RegulationHeading(&pwmL, &pwmR, &pwmB);
+ }
+ // 普通のときの処理
+ else {
+ // Theta = Angle * Byte2Rad;
+ direction *= Deg2Rad;
+ float vx = cos(direction);
+ float vy = sin(direction);
+
+ PwmLRB1(&pwmL, &pwmR, &pwmB, vx, vy, speed);
+ // PwmLRB2(&pwmL, &pwmR, &pwmB, vx, vy, speed);
+ // PwmLRB3(&pwmL, &pwmR, &pwmB, vx, vy, speed);
+ }
+
+ msgF(0, "pwmL", pwmL, false);
+ msgF(0, "pwmR", pwmR, false);
+ msgF(0, "pwmB", pwmB, false);
+
+ // 回転方向を決定 と マイナスだったらプラスに変える
+ uint8_t data0 = 0x3F; // 0011 1111
+ if (pwmL!=0.0 && pwmR!=0.0 && pwmB!=0.0) {
+ // 左モータ
+ if (pwmL>0) {
+ data0 &= 0x37; // 0011 0111
+ }
+ else {
+ data0 &= 0x3B; // 0011 1011
+ pwmL *= -1.0;
+ }
+ if (pwmL>1.0) pwmL=1.0;
+
+ // 右モータ
+ if (pwmR>0) {
+ data0 &= 0x3D; // 0011 1101
+ }
+ else {
+ data0 &= 0x3E; // 0011 1110
+ pwmR *= -1.0;
+ }
+ if (pwmR>1.0) pwmR=1.0;
+
+ // 後モータ
+ if (pwmB>0) {
+ data0 &= 0x1F; // 0001 1111
+ }
+ else {
+ data0 &= 0x2F; // 0010 1111
+ pwmB *= -1.0;
+ }
+ if (pwmB>1.0) pwmB=1.0;
+ }
+ else data0 = 0x00;
+
+ uint8_t dataL = (uint8_t)(pwmL * 255.0); // 左モータの回転数
+ uint8_t dataR = (uint8_t)(pwmR * 255.0); // 右モータの回転数
+ uint8_t dataB = (uint8_t)(pwmB * 255.0); // 後モータの回転数
+ if (dataL == 0x00) {
+ data0 &= 0x33; // 0011 0011
+ data0 |= 0x0C;
+ dataL++;
+ }
+ if (dataR == 0x00) {
+ data0 &= 0x3C; // 0011 1100
+ data0 |= 0x03;
+ dataR++;
+ }
+ if (dataB == 0x00) {
+ data0 &= 0x0F; // 0000 1111
+ data0 |= 0x30;
+ dataB++;
+ }
+
+ // データ確認用の値を埋め込む
+ dataL = dataL & 0xFC | 0x01;
+ dataR = dataR & 0xFC | 0x02;
+ dataB = dataB & 0xFC | 0x03;
+
+ // デバッグのため、送信データを表示
+ // pc.printf(" ");
+ msgX(0, "data0", data0, false);
+ msgX(0, "dataL", dataL, false);
+ msgX(0, "dataR", dataR, false);
+ msgX(0, "dataB", dataB, true);
+
+ // データを送信
+ int error = 0;
+ int count = 0;
+ while (1) {
+ // 通信開始の合図
+ cs_Motor=0; wait_us(t_SS_SCLK);
+ rx_data=spi.write((uint8_t)(error%4)); wait_us(t_SS_SCLK);
+ cs_Motor=1;
+ if (rx_data!=0xAA) {
+ errorMsgHex(0, "start", rx_data);
+ // errorMsgHex(0, "start_data", (uint8_t)(error%4));
+ }
+ wait_us(SPI_WAIT_US);
+ // 回転方向のデータ
+ do {
+ cs_Motor=0; wait_us(t_SS_SCLK);
+ rx_data=spi.write(data0); wait_us(t_SS_SCLK);
+ cs_Motor=1;
+ count++;
+ } while(rx_data != 0x00 && count < 5);
+ if (rx_data!=0x00) {
+ errorMsgHex(0, "data0", rx_data);
+ led_Motor = 0;
+ error++;
+ continue;
+ } else led_Motor = 1;
+ msgD(0, "count", count);
+ count = 0;
+ wait_us(SPI_WAIT_US);
+ // MotorLのデータ
+ do {
+ cs_Motor=0; wait_us(t_SS_SCLK);
+ rx_data=spi.write(dataL); wait_us(t_SS_SCLK);
+ cs_Motor=1;
+ count++;
+ } while(rx_data != data0 && count < 5);
+ if (rx_data!=data0) {
+ errorMsgHex(0, "dataL", rx_data);
+ led_Motor = 0;
+ error++;
+ continue;
+ } else led_Motor = 1;
+ msgD(0, "count", count);
+ count = 0;
+ wait_us(SPI_WAIT_US);
+ // MotorRのデータ
+ do {
+ cs_Motor=0; wait_us(t_SS_SCLK);
+ rx_data=spi.write(dataR); wait_us(t_SS_SCLK);
+ cs_Motor=1;
+ count++;
+ } while(rx_data != dataL && count < 5);
+ if (rx_data!=dataL) {
+ errorMsgHex(0, "dataR", rx_data);
+ led_Motor = 0;
+ error++;
+ continue;
+ } else led_Motor = 1;
+ msgD(0, "count", count);
+ count = 0;
+ wait_us(SPI_WAIT_US);
+ // MotorBのデータ
+ do {
+ cs_Motor=0; wait_us(t_SS_SCLK);
+ rx_data=spi.write(dataB); wait_us(t_SS_SCLK);
+ cs_Motor=1;
+ count++;
+ } while(rx_data != dataR && count < 5);
+ if (rx_data!=dataR) {
+ errorMsgHex(0, "dataB", rx_data);
+ led_Motor = 0;
+ error++;
+ continue;
+ } else led_Motor = 1;
+ msgD(0, "count", count);
+ count = 0;
+ wait_us(SPI_WAIT_US);
+
+ break;
+ }
+
+ return;
+}
+
+/*--- ボールの方向から、ロボットの進行方向を決める関数 ---*/
+inline float MoveDirection(int iBallDirection)
+{
+ /*---
+
+ - ボールの方向から、ロボットの進行方向を求める関数
+ - -180 < iBallDirection < +180
+
+ ---*/
+
+ if (iBallDirection == 1000.0) return 1000.0;
+
+ float fMoveDirection = 0.0;
+ if (-30<iBallDirection && iBallDirection<30) fMoveDirection = 0.0;
+ else if (iBallDirection > 0) fMoveDirection = iBallDirection+60.0;
+ else if (iBallDirection < 0) fMoveDirection = iBallDirection-60.0;
+ else fMoveDirection = 180.0;
+
+ // 角度を -180~+180 に修正
+ if (fMoveDirection > 180.0){
+ return fMoveDirection - 360.0;
+ }
+ if (fMoveDirection < -180.0){
+ return fMoveDirection + 360.0;
+ }
+
+ return fMoveDirection;
+}
+
+
+/*--- SPI通信確認 ---*/
+bool spiChecking()
+{
+ int spicheck = 0;
+ uint8_t data = 0;
+ uint8_t missData1 = 0;
+ uint8_t missData2 = 0;
+ uint8_t missData3 = 0;
+ uint8_t missData4 = 0;
+ pc.printf("\nChecking SPI...\n");
+
+ #ifdef PING_RLB_
+ cs_PingRLB = 0; missData1 = spi.write(0x50); cs_PingRLB = 1; wait_us(SPI_WAIT_US);
+ cs_PingRLB = 0; missData2 = spi.write(0x50); cs_PingRLB = 1; wait_us(SPI_WAIT_US);
+ cs_PingRLB = 0; data = spi.write(0x50); cs_PingRLB = 1;
+ if (data != 0xAA) {
+ pc.printf("##Bad## mbed <-> LPC1114 <-> Ping(RLB)\n");
+ pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data);
+ spicheck++;
+ }
+ #endif
+
+ #ifdef MOTOR_
+ led_Motor = 0;
+ // ちゃんと値が入っているかをチェック
+ cs_Motor = 0; missData1 = spi.write(0x51); cs_Motor = 1; wait_us(SPI_WAIT_US);
+ cs_Motor = 0; missData2 = spi.write(0x51); cs_Motor = 1; wait_us(SPI_WAIT_US);
+ cs_Motor = 0; missData3 = spi.write(0x51); cs_Motor = 1; wait_us(SPI_WAIT_US);
+ cs_Motor = 0; missData4 = spi.write(0x51); cs_Motor = 1; wait_us(SPI_WAIT_US);
+ cs_Motor = 0; data = spi.write(0x01); cs_Motor = 1;
+ pc.printf(" ReturnValue1 = %x\n ReturnValue2 = %x\n ReturnValue3 = %x\n ReturnValue4 = %x\n ReturnValue5 = %x\n", missData1, missData2, missData3, missData4, data);
+ // SPI通信のチェック
+ cs_Motor = 0; missData1 = spi.write(0x50); cs_Motor = 1; wait_us(SPI_WAIT_US);
+ cs_Motor = 0; missData2 = spi.write(0x50); cs_Motor = 1; wait_us(SPI_WAIT_US);
+ cs_Motor = 0; data = spi.write(0x01); cs_Motor = 1;
+ if (data != 0xAA) {
+ pc.printf("##Bad## mbed <-> CY8C29466 <-> Motor\n");
+ pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data);
+ spicheck++;
+ }
+ else led_Motor=1;
+ #endif
+
+ #ifdef KICK_DIV_
+ cs_KickerDribbler = 0; missData1 = spi.write(0x50); cs_KickerDribbler = 1; wait_us(SPI_WAIT_US);
+ cs_KickerDribbler = 0; missData2 = spi.write(0x50); cs_KickerDribbler = 1; wait_us(SPI_WAIT_US);
+ cs_KickerDribbler = 0;
+ data = spi.write(0x01);
+ cs_KickerDribbler = 1;
+ if (data != 0xAA) {
+ pc.printf("##Bad## mbed <-> CY8C29466 <-> Kicker&Dribbler\n");
+ pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data);
+ spicheck++;
+ }
+ #endif
+
+ #ifdef IR_FR_
+ cs_IRFrDistance = 0; missData1 = spi.write(0x50); cs_IRFrDistance = 1; wait_us(SPI_WAIT_US);
+ cs_IRFrDistance = 0; missData2 = spi.write(0x50); cs_IRFrDistance = 1; wait_us(SPI_WAIT_US);
+ cs_IRFrDistance = 0; data = spi.write(0x01); cs_IRFrDistance = 1;
+ if (data != 0xAA) {
+ pc.printf("##Bad## mbed <-> CY8C24123A <-> IR_FR_distance\n");
+ pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data);
+ spicheck++;
+ }
+ #endif
+
+ #ifdef IR_BR_
+ cs_IRBrDistance = 0; missData1 = spi.write(0x50); cs_IRBrDistance = 1; wait_us(SPI_WAIT_US);
+ cs_IRBrDistance = 0; missData2 = spi.write(0x50); cs_IRBrDistance = 1; wait_us(SPI_WAIT_US);
+ cs_IRBrDistance = 0; data = spi.write(0x01); cs_IRBrDistance = 1;
+ if (data != 0xAA) {
+ pc.printf("##Bad## mbed <-> CY8C24123A <-> IR_BR_distance\n");
+ pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data);
+ spicheck++;
+ }
+ #endif
+
+ #ifdef IR_BL_
+ cs_IRBlDistance = 0; missData1 = spi.write(0x50); cs_IRBlDistance = 1; wait_us(SPI_WAIT_US);
+ cs_IRBlDistance = 0; missData2 = spi.write(0x50); cs_IRBlDistance = 1; wait_us(SPI_WAIT_US);
+ cs_IRBlDistance = 0; data = spi.write(0x01); cs_IRBlDistance = 1;
+ if (data != 0xAA) {
+ pc.printf("##Bad## mbed <-> CY8C24123A <-> IR_BL_distance\n");
+ pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data);
+ spicheck++;
+ }
+ #endif
+
+ #ifdef IR_FL_
+ cs_IRFlDistance = 0; missData1 = spi.write(0x50); cs_IRFlDistance = 1; wait_us(SPI_WAIT_US);
+ cs_IRFlDistance = 0; missData2 = spi.write(0x50); cs_IRFlDistance = 1; wait_us(SPI_WAIT_US);
+ cs_IRFlDistance = 0; data = spi.write(0x01); cs_IRFlDistance = 1;
+ if (data != 0xAA) {
+ pc.printf("##Bad## mbed <-> CY8C24123A <-> IR_FL_distance\n");
+ pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data);
+ spicheck++;
+ }
+ #endif
+
+ #ifdef IR__
+ led_IR = 0;
+ cs_IR = 0; missData1 = spi.write(0x50); cs_IR = 1; wait_us(SPI_WAIT_US);
+ cs_IR = 0; missData2 = spi.write(0x50); cs_IR = 1; wait_us(SPI_WAIT_US);
+ cs_IR = 0; data = spi.write(0x01); cs_IR = 1;
+ if (data != 0xAA) {
+ pc.printf("##Bad## mbed <-> CY8C29466 <-> IR_direction\n");
+ pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data);
+ spicheck++;
+ }
+ else led_IR=1;
+ #endif
+
+ #ifdef PING_F_
+ cs_PingF = 0; missData1 = spi.write(0x50); cs_PingF = 1; wait_us(SPI_WAIT_US);
+ cs_PingF = 0; missData2 = spi.write(0x50); cs_PingF = 1; wait_us(SPI_WAIT_US);
+ cs_PingF = 0; data = spi.write(0x01); cs_PingF = 1;
+ if (data != 0xAA) {
+ pc.printf("##Bad## mbed <-> LPC1114 <-> Ping(F)\n");
+ pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data);
+ spicheck++;
+ }
+ #endif
+
+ #ifdef IMU_
+ led_IMU = 0;
+ int count = 5;
+ do {
+ cs_IMU = 0; data = spi.write(0x50); cs_IMU = 1; wait_us(SPI_WAIT_US);
+ pc.printf("data : %x\n", data);
+ count--;
+ } while ((data==0xFF || data==0x00) && count);
+
+ if (count < 1) {
+ spicheck++;
+ pc.printf("##Bad## mbed <-> LPC1114 <-> LSM9DS0\n");
+ }
+ else led_IMU=1;
+ pc.printf(" IMU : %d\n", data);
+ #endif
+
+ #ifdef DEBUG_
+ cs_Debug = 0; missData1 = spi.write(0x50); cs_Debug = 1; wait_us(SPI_WAIT_US);
+ cs_Debug = 0; missData2 = spi.write(0x50); cs_Debug = 1; wait_us(SPI_WAIT_US);
+ cs_Debug = 0; data = spi.write(0x01); cs_Debug = 1;
+ if (data != 0xAA) {
+ pc.printf("##Bad## mbed <-> LPC1114_DEBUG\n");
+ pc.printf(" MissData1 = %x\n MissData2 = %x\n MissData3 = %x\n", missData1, missData2, data);
+ spicheck++;
+ }
+ #endif
+
+ // 最後にメッセージを表示
+ if (spicheck) {
+ pc.printf("waring : %d\n", spicheck);
+ led4 = 0;
+ // while (!pc.readable()); // PCから指示があるまで待つ
+ // pc.getc();
+ return 0;
+ wait(2);
+ }
+ else {
+ pc.printf("All SPIdevices complete!!\n");
+ led4 = 1;
+ return 1;
+ wait(1);
+ }
+ pc.printf("\n");
+}
+
+
+/*--- PCとの通信用関数 ---*/
+inline void ControlCommand(uint8_t command)
+{
+ /*--- コマンド一覧 ---
+ * 必要なparameter
+ 0x01 PID_Kp PID制御の比例
+ 0x02 PID_Ki PID制御の積分
+ 0x03 PID_Kd PID制御の微分
+ 0x04 Robo_R 機体の半径
+
+ * 必要なcommand
+ 0x10 modeStop 停止コマンド
+ 0x11 modeBattle1 戦闘モード1
+ 0x12 modeBattle2 戦闘モード2
+ 0x13 modeDebug デバッグモード
+ 0x21 goFlont 前進
+ 0x22 goBack 後退
+ 0x23 goLeft 左
+ 0x24 goRight 右
+ 0x25 turnCW 時計回りに一回転
+ 0x26 turnCCW 反時計回りに一回転
+ 0x31 checkSensor センサーチェック
+ 0x31 checkIMU IMUからの情報を画面に表示
+ 0xAA Reset リセット（SDカードから、値を再読み込み）
+ ---*/
+ float wait_time = 1.0;
+ switch (command)
+ {
+ case 0xAA:
+ break;
+ case 0x13: // modeDebug
+ spiChecking();
+ wait(3);
+ break;
+ case 0x21: // goFlont
+ Motor(0.0, 0.3);
+ wait(wait_time);
+ break;
+ case 0x22: // goBack
+ Motor(180.0, 0.3);
+ wait(wait_time);
+ break;
+ case 0x23: // goLeft
+ Motor(-90.0, 0.3);
+ wait(wait_time);
+ break;
+ case 0x24: // goRight
+ Motor(90.0, 0.3);
+ wait(wait_time);
+ break;
+ default:
+ break;
+ }
+}
+
+
+#ifdef XBEE
+/*--- 送信用関数 ---*/
+void xbeeTx(uint8_t *address, uint8_t data) {
+ int i=0;
+
+ xbee.putc(0x7E);
+ xbee.putc(0x00); // フレーム長
+ xbee.putc(0x0F);
+ xbee.putc(0x10); // フレームタイプ
+ xbee.putc(0x01); // フレームID
+ for (int i=0; i<8; i++)
+ xbee.putc(address[i]); // 62bit宛先アドレス
+ xbee.putc(0xFF); // 16bit宛先ネットワークアドレス
+ xbee.putc(0xFE);
+ xbee.putc(0x00); // ブロードキャスト半径
+ xbee.putc(0x00); // オプション
+ xbee.putc(data); // RFデータ
+ // チェックサムの計算
+ uint8_t checksum = 0xFF - (0x10+0x01+0xFF+0xFE+data);
+ for (i=0; i<8; i++) checksum -= address[i];
+ xbee.putc(checksum); // チェックサム
+
+ // 送信ステータスの確認
+ if (xbee.readable()) {
+ if (xbee.getc() == 0x7E) {
+ for (i=0; i<8; i++) xbee.getc();
+ if (xbee.getc() == 0x00)
+ // pc.printf("Send success!\n");
+ else
+ // pc.printf("Send missed.\n");
+ for (i=9; i<10; i++) xbee.getc();
+ }
+ }
+}
+/*--- 受信用関数 ---*/
+xbeeAPI xbeeRx() {
+ xbeeAPI returnApi; // 返り値用の変数
+ returnApi.reset();
+ if (xbee.readable() > 0) {
+ // pc.printf("readable\n");
+ if (xbee.getc() == 0x7E) {
+ // pc.printf("startBit\n");
+ int i=0;
+ uint8_t buffer[18];
+
+ buffer[0] = 0x7E;
+ for (i=1; i<18; i++) {
+ buffer[i] = xbee.getc();
+ }
+ // 送信元アドレスを抽出
+ for (i=0; i<8; i++) {
+ returnApi.address[i] = buffer[i+4];
+ }
+ // 受信データを抽出
+ returnApi.direction = buffer[15];
+ returnApi.turning = buffer[16];
+
+ // データをPCに表示
+ // for (i=0; i<8; i++) pc.printf("%x", returnApi.address[i]);
+ // pc.printf("\n");
+ // pc.printf("%x\n", returnApi.direction);
+ // pc.printf("%x\n", returnApi.turning);
+ }
+ }
+ return returnApi;
+}
+#endif
+
+/*--- serialの通信チェック関数 ---*/
+inline void serialChecking()
+{
+ if ( pc.readable() ) {
+ uint8_t command = pc.getc();
+ printf("command : %x\n", pc.getc());
+ ControlCommand(command);
+ }
+}
+
+
+
+
+int main()
+{
+ /*--- 変数宣言 ---*/
+ int direction = 0; // ボールのある方向
+ float speed = 0.0; // ロボットの速度
+
+ /*--- 初期処理 ---*/
+ // 音
+ float mm[]={mC,mD,mE,mF,mG,mA,mB,mC*2}; // ドレミファソラシ
+ sound.period(1.0/mm[0]);
+ sound = 0.03;
+ // Serialの設定
+ pc.baud(115200);
+ // SPIの初期設定
+ spi.format(8, 3); // DataFlameのビット数(4~16)
+ spi.frequency(1000000); // クロック周波数(デフォルト:1MHz)
+ // ラインセンサに許可信号を
+ linePermission=1;
+ wait(1); // 各モジュールのセットアップが完了するまで待つ
+
+ // PIDライブラリの初期設定
+ pidController.setInputLimits(-128.0, 127.0);
+ pidController.setOutputLimits(-1.0, 1.0);
+ pidController.setBias(0.0);
+ pidController.setMode(AUTO_MODE);
+ pidController.setSetPoint(0.0);
+ // pidupdata.attach(&ImuRead, RATE);
+
+ // lineの設定
+ // lineLeft.rise(&lineMove);
+ // lineRight.rise(&lineMove);
+
+ // SPIの通信確認
+ while ( !spiChecking() );
+
+
+ /*--- スタートの合図を待つ ---*/
+ kickerOuput = 1;
+ // SWが押されるまで待つ
+ sound.period(1.0/mm[1]);
+ sound = 0.06;
+ while(!SWcheck());
+ // 音を止める
+ sound = 0.00;
+
+
+ /*--- メイン処理 ---*/
+ while(true) {
+
+ #ifdef PROG_TIME
+ t.start();
+ #endif
+
+ /*--- pcとの通信があったかをCheck ---*/
+ // serialChecking();
+ /*--- ストップボタンをCheck ---*/
+ if (SWcheck()) {
+ Motor(0.0, 0.0);
+ sound.period(1.0/mm[1]);
+ sound = 0.06;
+ while(!SWcheck());
+ sound = 0.00;
+ }
+ lineCheck();
+
+ // PingRead();
+
+ /*--- 現在の方角を確認 ---*/
+ // Kicker使用時は、IMUの値は参考にならないので無視する
+ // if (kickerInput) {
+ // kickerOuput = 1;
+ // wait(0.8);
+ // kickerOuput = 0;
+ // } else {
+ // ImuRead();
+ // pidController.setProcessValue(Angle);
+ // }
+ ImuRead();
+ pidController.setProcessValue(Angle);
+ lineCheck();
+
+ /*--- ロボットから見たボールの方向を確認 ---*/
+ direction = IRRead();
+ lineCheck();
+
+ /*--- ボールの方向の絶対値を算出 ---*/
+ if (direction != 1000.0) {
+ direction += Angle;
+ if (direction > 180.0){
+ direction -= 360.0;
+ }
+ else if (direction < -180.0){
+ direction += 360.0;
+ }
+ }
+ lineCheck();
+
+ /*--- ロボットの進行方向を決定 ---*/
+ moveDirection = MoveDirection(direction);
+ // pc.printf("%f\n", moveDirection);
+ lineCheck();
+
+ /*--- 修正のための角速度を算出 ---*/
+ // GetOmega();
+ Omega = pidController.compute();
+ if (-5<Angle && Angle<5) {
+ Omega = 0.0;
+ }
+ lineCheck();
+
+ /*--- 実際にロボットを動かす ---*/
+ // moveDirection = 0.0;
+ // moveDirection = 1000.0;
+ Motor(moveDirection, 0.4);
+ lineCheck();
+
+ Direction = moveDirection;
+
+
+ #ifdef PROG_TIME
+ t.stop();
+ pc.printf("%f\n", t.read());
+ t.reset();
+ #endif
+
+ LoopCount++;
+ }
+}
+
+
+
+
+
+
+
+
+
+
+
+