NITICRobotClub 2018Team-B
NHK2018Bチームの手動ロボットのプログラムです。（製作中）
Dependencies: HCSR04 PID PS3viaSBDBT QEI mbed
Diff: main.cpp
- Revision:
- 1:5609d9509abf
- Parent:
- 0:1ebf4907639c
- Child:
- 2:235db39e0eaa
--- a/main.cpp Sat Sep 22 11:02:36 2018 +0000
+++ b/main.cpp Sun Sep 23 13:38:03 2018 +0000
@@ -10,17 +10,16 @@
/* 使用センサ:ロータリーエンコーダ: 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
+#define roller_Kp 4.0
+#define froller_Ki 0.1
+#define broller_Ki 0.1
+#define roller_Kd 0.05
//定義
@@ -38,7 +37,6 @@
*/
I2C i2c(PB_9, PB_8);
//PS3コン定義
-PS3s ps3s(PC_10, PC_11);
//PC定義
Serial pc(USBTX,USBRX);
//ロリコン定義
@@ -102,6 +100,7 @@
void flip();
//ローラーPID
int roller_PID(int front, int back);
+int sec;
//発射関数
void shoot();
//その他諸関数
@@ -109,95 +108,48 @@
void printdata();
void outemergency();
void emergency();
+void TimerIsr();
+Ticker timer_;
//実装部
//メイン関数
int main()
{
- float looptime;
+ //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();
+ /*
+ TimerIsr();
+ timer_.attach(&TimerIsr, 1);
+ */
+ while(true){
//pc.printf("\x1b[0m");
- pc.printf("%3d,%3d,%f\n\r", abs(front_roller_rpm), abs(back_roller_rpm), looptime);
- mainloop.reset();
- mainloop.start();
+ roller_PID(1550,770);
+ topsonic.start();
+ wait(0.01);
+ sonicval = topsonic.get_dist_cm();
+
+ pc.printf("%3d,%3d,%d\n\r", abs(front_roller_rpm), abs(back_roller_rpm),sonicval);
+
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 TimerIsr()
+{
+ static int k = 0;
+ sec = k % 60; // seconds
+ k++;
+}
+*/
//メイン制御関数
-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()
{
@@ -212,165 +164,6 @@
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()
{
@@ -466,50 +259,3 @@
}
}
}
-
-
-//セミオートプレイテスト
-
-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