NITICRobotClub 2018Team-B
NHK2018Bチームの手動ロボットのプログラムです。（製作中）
Dependencies: HCSR04 PID PS3viaSBDBT QEI mbed
Diff: main.cpp
- Revision:
- 0:1ebf4907639c
- Child:
- 1:5609d9509abf
- Child:
- 3:f462b1244d77
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Sep 22 11:02:36 2018 +0000
@@ -0,0 +1,515 @@
+/* ------------------------------------------------------------------- */
+/* NHKロボコン2018-Bチーム手動ロボット(設計者: 4S 関) */
+/* ------------------------------------------------------------------- */
+/* このプログラムは上記のロボット専用の制御プログラムである。 */
+/* ------------------------------------------------------------------- */
+/* 対応機種: NUCLEO-F446RE */
+/* ------------------------------------------------------------------- */
+/* 製作者: 1-3 武井 智大, mail: taobmcoe@outlook.com */
+/* ------------------------------------------------------------------- */
+/* 使用センサ:ロータリーエンコーダ: 2個, 超音波センサ: 1個*/
+/* ------------------------------------------------------------------- */
+#include "mbed.h"
+#include "PS3_S.h"
+#include "hcsr04.h"
+#include "QEI.h"
+#include "PID.h"
+
+#define ps3data ps3s.condata
+#define PI 3.14159265359
+#define roller_Kp 5.0
+#define froller_Ki 0.0
+#define broller_Ki 0.0
+#define roller_Kd 0.0
+
+//定義
+
+/*I2C定義
+ 0前輪左:0x10
+ 1前輪右:0x12
+ 2後輪左:0x14
+ 3後輪右:0x16
+ 4投射前:0x18
+ 5投射後ろ:0x20
+ 6エアシリンダー:0x40
+ 正転:0x84~0xFF
+ 逆転:0x00~0x7C
+ ショートブレーキ:0x7D~0x83
+*/
+I2C i2c(PB_9, PB_8);
+//PS3コン定義
+PS3s ps3s(PC_10, PC_11);
+//PC定義
+Serial pc(USBTX,USBRX);
+//ロリコン定義
+QEI front_roller_wheel(PC_8, PC_6, NC, 624);
+QEI back_roller_wheel(PC_5, PA_12, NC, 624);
+//PID定義
+PID front_roller(roller_Kp, froller_Ki, roller_Kd, 0.001);
+PID back_roller(roller_Kp, broller_Ki, roller_Kd, 0.001);
+//超音波センサ定義
+HCSR04 topsonic(PB_2,PB_1);
+//HCSR04 undersonic(PB_15,PB_14);
+//LED定義
+DigitalOut Power(PC_12);//Green
+DigitalOut i2ccheck(PB_7);//Blue
+DigitalOut sncled(PC_2);//Yellow
+DigitalOut Powemer(PC_3);//Red
+DigitalOut myled(LED1);
+//緊急停止定義
+DigitalOut emersig(PC_0);
+//タイマー定義
+Ticker get_rps;//ロリコンからRPMの読み取り
+Timer shootpet;
+Timer mainloop;
+
+InterruptIn but(USER_BUTTON);
+
+//変数定義
+//データ配列
+char send_data[4][1];//足回り用データ配列
+char send_data2[1];//エアシリンダー用データ配列
+int address[4] = {0x10, 0x12, 0x14, 0x16};//i2cアドレス
+int err[4] = {1, 1, 1, 1};//I2Cリザルト
+
+int front_roller_rpm;//RPM変数
+int back_roller_rpm;
+int front_roller_pulse[10];//ロリコンパルスバッファ
+int back_roller_pulse[10];
+int sum_front_roller_pulse;//ロリコンパルス
+int sum_back_roller_pulse;
+int ave_front_roller_pulse;//ロリコンパルス平均
+int ave_back_roller_pulse;
+char front_roller_data[1];
+char back_roller_data[1];
+int dmove_val;
+static int j = 0;
+int sonicval;
+int mode = true;
+
+//プロトタイプ宣言
+//メイン制御関数
+void ctrl();
+void semiauto();
+//移動関数
+void amove_R();
+void amave_L();
+void dmove();
+//void shortb(int i);
+void backfif();
+void sendi2c();
+//回転数取得関数
+void flip();
+//ローラーPID
+int roller_PID(int front, int back);
+//発射関数
+void shoot();
+//その他諸関数
+void changemode();
+void printdata();
+void outemergency();
+void emergency();
+
+//実装部
+
+//メイン関数
+int main()
+{
+ float looptime;
+ pc.baud(460800);
+ emersig = 0;
+ Power = 1;
+ i2ccheck = 0;
+ get_rps.attach_us(&flip, 40000);
+ but.rise(shoot);
+ while(true) {
+ looptime = mainloop.read();
+ //pc.printf("\x1b[0m");
+ pc.printf("%3d,%3d,%f\n\r", abs(front_roller_rpm), abs(back_roller_rpm), looptime);
+ mainloop.reset();
+ mainloop.start();
+ i2c.write(0x18, front_roller_data, 1, false);
+ i2c.write(0x20, back_roller_data, 1, false);
+ topsonic.start();
+ wait(0.01);
+ sonicval = topsonic.get_dist_cm();
+ ps3s.getdata();
+ if(ps3s.endval == 0){
+ myled = 1;
+ }else{
+ myled = 0;
+
+ }
+ //pc.printf("%d", ps3s.databuffer);
+ ps3s.checkdata();
+ ctrl();
+ printdata();
+ mainloop.stop();
+ }
+}
+
+//メイン制御関数
+void ctrl()
+{
+
+ if(ps3data[12]!=0) {//Select
+ emergency();//緊急停止
+ } else if(ps3data[13]!=0) {//Start
+ outemergency();//緊急停止解除
+ }
+
+ if(emersig == 0) {
+ for(int i = 0; i < 4; i++) {
+ send_data[i][0] = 0x83;//ショートブレーキ
+ }
+
+ if(ps3data[0]!=0) {
+ dmove_val += 1;//上
+ } else if(ps3data[1]!=0) {
+ dmove_val += 2;//下
+ } else if(ps3data[2]!=0) {
+ dmove_val += 4;//右
+ } else if(ps3data[3]!=0) {
+ dmove_val += 8;//左
+ } else if(ps3data[8]!=0) {//L1
+ dmove_val += 32;//左回転
+ } else if(ps3data[10]!=0) {//R1
+ dmove_val += 16;//右回転
+ }
+ if (dmove_val != 0) {
+ dmove();//方向キー+L1R1キー
+ }
+
+ if(((ps3data[16] <= 51) || (ps3data[16] >= 77)) || ((ps3data[17] <= 51) || (ps3data[17] >= 77))) {
+ amove_R();//アナログパッド
+ }
+
+ if(ps3data[7]!=0) {//しかく
+ backfif();//50cm後退
+ } else if(ps3data[6]!=0) {//まる
+ shoot();//投擲
+ } else if(ps3data[4]!=0) { //さんかく
+ semiauto();
+ } else if(ps3data[5]!=0) { //しかく
+ changemode();
+ }
+ sendi2c();
+ } else {
+ }
+}
+
+//緊急停止
+void emergency()
+{
+ emersig = 1;
+ Powemer = 1;
+}
+
+//緊急停止解除
+void outemergency()
+{
+ emersig = 0;
+ Powemer = 0;
+}
+
+//右アナログパッド
+void amove_R()
+{
+ int RXdata = ps3data[16];
+ int RYdata = ps3data[17];
+
+ if(RXdata == 64 && RYdata < 64) {
+ //垂直上
+ send_data[0][0] = abs(RYdata - 53) + 202 - RYdata - 8;
+ send_data[1][0] = abs(RYdata - 53) + 202 - RYdata;
+ send_data[2][0] = abs(RYdata - 53) + 202 - RYdata - 8;
+ send_data[3][0] = abs(RYdata - 53) + 202 - RYdata;
+
+ } else if(RXdata == 64 && RYdata > 64) {
+ //垂直下
+ send_data[0][0] = abs(RYdata - 127) * 2;
+ send_data[1][0] = abs(RYdata - 127) * 2 + 8;
+ send_data[2][0] = abs(RYdata - 127) * 2;
+ send_data[3][0] = abs(RYdata - 127) * 2 + 8;
+
+ } else if(RXdata > 64 && RYdata == 64) {
+ //垂直右
+ send_data[0][0] = (RXdata - 75) + 203 - abs(RXdata - 127) - 8;
+ send_data[3][0] = (RXdata - 75) + 203 - abs(RXdata - 127) - 8;
+ send_data[1][0] = abs(RXdata - 127) * 2;
+ send_data[2][0] = abs(RXdata - 127) * 2;
+
+ } else if(RXdata < 64 && RYdata == 64) {
+ //垂直左
+ send_data[0][0] = RXdata * 2;
+ send_data[3][0] = RXdata * 2;
+ send_data[1][0] = abs(RXdata - 53) + 202 - RXdata - 8;
+ send_data[2][0] = abs(RXdata - 53) + 202 - RXdata - 8;
+
+ } else if (RXdata > 64 && RYdata < 64) {
+ //右上
+ send_data[0][0] = (RXdata - 74) + 202 - abs(RXdata - 127);
+ send_data[3][0] = (RXdata - 74) + 202 - abs(RXdata - 127);
+ } else if(RXdata < 64 && RYdata < 64) {
+ //左上
+ send_data[1][0] = abs(RXdata - 54) + 201 - RXdata;
+ send_data[2][0] = abs(RXdata - 54) + 201 - RXdata;
+ } else if(RXdata < 64 && RYdata > 64) {
+ //左下
+ send_data[0][0] = RXdata * 2;
+ send_data[3][0] = RXdata * 2;
+ } else if(RXdata > 64 && RYdata > 64) {
+ //右下
+ send_data[1][0] = abs(RXdata - 127) * 2;
+ send_data[2][0] = abs(RXdata - 127) * 2;
+ }
+}
+
+void dmove()
+{
+ int k = 32;
+ while(true) {
+ if (dmove_val == 0) {
+ break;
+ } else {
+ if (dmove_val - k >= 0) {
+ sncled = 1;
+ switch (k) {
+ case 1:
+ send_data[0][0] = 0xF7;
+ send_data[1][0] = 0xFF;
+ send_data[2][0] = 0xF7;
+ send_data[3][0] = 0xFF;
+ break;
+ case 2:
+ send_data[0][0] = 0x00;
+ send_data[1][0] = 0x0A;
+ send_data[2][0] = 0x00;
+ send_data[3][0] = 0x0A;
+ break;
+ case 4:
+ send_data[0][0] = 0xF5;
+ send_data[3][0] = 0xFF;
+ send_data[1][0] = 0x09;
+ send_data[2][0] = 0x00;
+ break;
+ case 8:
+ send_data[0][0] = 0x00;
+ send_data[3][0] = 0x09;
+ send_data[1][0] = 0xFF;
+ send_data[2][0] = 0xF5;
+ break;
+ case 16:
+ send_data[1][0] = 0x00;
+ send_data[3][0] = 0x00;
+ send_data[0][0] = 0xFF;
+ send_data[2][0] = 0xFF;
+ break;
+ case 32:
+ send_data[0][0] = 0x00;
+ send_data[2][0] = 0x00;
+ send_data[1][0] = 0xFF;
+ send_data[3][0] = 0xFF;
+ break;
+ default:
+ break;
+ }
+ sncled = 0;
+ dmove_val -= k;
+ } else {
+ k /= 2;
+ }
+ }
+ }
+}
+
+//ショートブレーキ
+void shortb(int i)
+{
+ i2ccheck = 0;
+ send_data[i][0] = 0x83;
+}
+
+//50cm後退
+void backfif()
+{
+ int snc1;
+ //int snc2;
+ while(true) {
+ topsonic.start();
+ //undersonic.start();
+ snc1 = topsonic.get_dist_cm();
+ //snc2 = undersonic.get_dist_cm();
+ if (snc1 != 0) { // && (snc2 != 0)) {
+ break;
+ }
+ }
+
+ int snc1buffer;
+ //int snc2buffer;
+ while(true) {
+ topsonic.start();
+ //undersonic.start();
+ snc1buffer = topsonic.get_dist_cm();
+ //snc2buffer = undersonic.get_dist_cm();
+ if (snc1buffer == 0) { // || snc2buffer == 0) {
+ continue;
+ }
+ if(snc1buffer >= snc1 + 50) {//|| (snc2buffer >= snc2 + 50)) {
+ sncled = 1;
+ for(int i = 0; i < 4; i++) {
+ send_data[i][0] = 0x83;
+ }
+ break;
+ } else {
+ for(int i = 0; i < 4; i++) {
+ send_data[i][0] = 0x10;
+ i2c.write(address[i], send_data[i], 1);
+ pc.printf("Goal:%dcm,Now:%dcm\n\r", snc1 + 50, snc1buffer);
+ }
+ }
+ }
+}
+
+//I2C送信
+void sendi2c()
+{
+ err[0] = i2c.write(address[0], send_data[0], 1);
+ err[1] = i2c.write(address[1], send_data[1], 1);
+ err[2] = i2c.write(address[2], send_data[2], 1);
+ err[3] = i2c.write(address[3], send_data[3], 1);
+ if (err[0]!=0 || err[1]!=0 || err[2]!=0 || err[3]!=0) {
+ while(10) {
+ i2ccheck = 1;
+ wait(0.1);
+
+ i2ccheck = 0;
+ }
+ } else {
+ i2ccheck = 1;
+ }
+}
+
+//移動平均をPID
+void flip()
+{
+ //前回のi番目のデータを消して新たにそこにデータを書き込む(キューのような考え?)
+ sum_front_roller_pulse -= front_roller_pulse[j];
+ sum_back_roller_pulse -= back_roller_pulse[j];
+
+ //配列のi番目の箱に取得パルスを代入
+ front_roller_pulse[j] = front_roller_wheel.getPulses();
+ back_roller_pulse[j] = back_roller_wheel.getPulses();
+
+ front_roller_wheel.reset();
+ back_roller_wheel.reset();
+
+ //i[0]~i[9]までの合計値を代入
+ sum_front_roller_pulse += front_roller_pulse[j];
+ sum_back_roller_pulse += back_roller_pulse[j];
+
+ //インクリメント
+ j++;
+ //iが最大値(9)になったらリセット
+ if(j > 9) {
+ j = 0;
+ }
+ //10回分の合計値を10で割り、取得パルスの平均を出す
+ ave_front_roller_pulse = sum_front_roller_pulse / 10;
+ ave_back_roller_pulse = sum_back_roller_pulse / 10;
+
+ //平均値をRPMへ変換
+ front_roller_rpm = ave_front_roller_pulse * 25 * 60 / 1200;
+ back_roller_rpm = ave_back_roller_pulse * 25 * 60 / 1200;
+}
+
+//ローラー
+int roller_PID(int front, int back)
+{
+ front_roller.setInputLimits(-2000, 2000);
+ back_roller.setInputLimits(-2000, 2000);
+
+ front_roller.setOutputLimits(0x84, 0xFF);
+ back_roller.setOutputLimits(0x84, 0xFF);
+
+ front_roller.setMode(AUTO_MODE);
+ back_roller.setMode(AUTO_MODE);
+
+ front_roller.setSetPoint(front);
+ back_roller.setSetPoint(back);
+
+ front_roller.setProcessValue(abs(front_roller_rpm));
+ back_roller.setProcessValue(abs(back_roller_rpm));
+
+ front_roller_data[0] = front_roller.compute();
+ back_roller_data[0] = back_roller.compute();
+
+ return 0;
+}
+
+//投擲動作
+void shoot()
+{
+ int time;
+ shootpet.start();
+ while(true) {
+ time = shootpet.read();
+ if(time <= 0.5) {
+ send_data2[0] = 0x0F;
+ i2c.write(0x40, send_data2, 1);
+ } else {
+ send_data2[0] = 0x80;
+ i2c.write(0x40, send_data2, 1);
+ shootpet.stop();
+ shootpet.reset();
+ break;
+ }
+ }
+}
+
+
+//セミオートプレイテスト
+
+void semiauto()
+{
+ int table = 0;
+ if(mode == true) {
+ pc.printf("\x1b[41m");
+ } else if(mode == false) {
+ pc.printf("\x1b[44m");
+ }
+ while(true) {
+ wait(0.5);
+ if(ps3data[4] == 1) {
+ break;
+ }
+ topsonic.start();
+ wait(0.01);
+ sonicval = topsonic.get_dist_cm();
+ switch(table){
+ case 0:
+ roller_PID(1000, 666);
+ case 1:
+ roller_PID(666, 666);
+ }
+ i2c.write(0x18, front_roller_data, 1, false);
+ i2c.write(0x20, back_roller_data, 1, false);
+ if(sonicval < 1000) {
+
+ wait(0.01);
+ }
+ }
+}
+
+ void changemode() {
+ mode = !mode;
+ }
+
+
+//デバック用データ出力
+ void printdata() {
+ for (int i = 0; i < 7; i++) {
+ pc.printf("%d,",ps3s.Rdata[i]);
+ }
+ pc.printf("%d\n\r",ps3s.Rdata[7]);
+ }
\ No newline at end of file