INCTRC Information Sharing Test
Bチーム自動ロボットの足回りプログラム(2018/07/10時点) PIDかけて一方向に進むだけのプログラムでござんす。
Revision 1:62b321f6c9c3, committed 2018-07-15
- Comitter:
- yuron
- Date:
- Sun Jul 15 23:56:24 2018 +0000
- Parent:
- 0:f73c1b076ae4
- Commit message:
- B??????????????????(2018/07/16??) PID??????????????MD????????????????; ;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Jul 10 13:40:10 2018 +0000
+++ b/main.cpp Sun Jul 15 23:56:24 2018 +0000
@@ -23,8 +23,9 @@
PID LF(0.5, 0.3, 0.0, 0.001);
PID LB(0.5, 0.3, 0.0, 0.001);
I2C i2c(PB_3, PB_10); //SDA, SCL
-//Serial pc(USBTX, USBRX);
+Serial pc(USBTX, USBRX);
DigitalOut emergency(PA_13);
+DigitalOut myled(D13);
/* 以下446基板用 */
QEI wheel_LB(PA_1, PA_0, NC, 624);
@@ -34,6 +35,7 @@
Ticker get_RPS;
Ticker print_RPS;
+Timer timer;
int rps_RF;
int rps_RB;
@@ -77,6 +79,30 @@
/* RPS表示関数 */
void flip2();
+/*
+void ice(int address, int data)
+{
+ //周波数: 100kHz
+ //i2c.frequency(100000);
+
+ // スタートコンディション出力
+ i2c.start();
+ wait_us(20);
+
+ // アドレスの書き込み
+ i2c.write(address);
+ wait_us(20);
+
+ // データの書き込み
+ i2c.write(data);
+ wait_us(20);
+
+ // ストップコンディション出力
+ i2c.stop();
+ wait_us(90);
+}
+*/
+
void flip(){
pulse_RF = wheel_RF.getPulses();
@@ -143,7 +169,7 @@
*/
//pc.printf("RF: %d RB: %d LF: %d LB: %d\n", rpm_RF, rpm_RB, rpm_LF, rpm_LB);
- //pc.printf("%d\n", abs(rpm_RF));
+ //pc.printf("%d\n", rpm_RF);
//pc.printf("%lf\n", n_v);
//pc.printf("%lf\n", h_v);
//pc.printf("rpm: %d n_v: %lf\n", rpm[1], n_v);
@@ -158,12 +184,12 @@
{
//pc.printf("RPS_RB: %d RPS_LB: %d\n", abs(rps[0]), abs(rps[1]));
//pc.printf("RPM_RB: %d RPM_LB: %d\n", abs(rpm[0]), abs(rpm[1]));
- //pc.printf("%d\n", rpm[1]);
+ //pc.printf("%d\n", rpm_RF);
//pc.printf("%lf\n", a_v);
//pc.printf("rpm: %d a_v: %lf\n", rpm[1], a_v);
}
-void PID()
+void front_PID()
{
// センサ出力値の最小、最大
//RF.setInputLimits(0, 1100);
@@ -184,6 +210,11 @@
LF.setMode(AUTO_MODE);
LB.setMode(AUTO_MODE);
+ //RF: -
+ //RB: -
+ //LF: +
+ //LB: +
+
// 目標値
RF.setSetPoint(300);
RB.setSetPoint(300);
@@ -191,10 +222,10 @@
LB.setSetPoint(300);
// センサ出力
- RF.setProcessValue(abs(rpm_RF));
- RB.setProcessValue(abs(rpm_RB));
- LF.setProcessValue(abs(rpm_LF));
- LB.setProcessValue(abs(rpm_LB));
+ RF.setProcessValue(rpm_RF);
+ RB.setProcessValue(rpm_RB);
+ LF.setProcessValue(rpm_LF);
+ LB.setProcessValue(rpm_LB);
// 制御量(計算結果)
RF_data[0] = RF.compute();
@@ -209,6 +240,54 @@
true_LB_data[0] = 0x38 - LB_data[0];
}
+void back_PID()
+{
+ // センサ出力値の最小、最大
+ //RF.setInputLimits(0, 1100);
+ RF.setInputLimits(-400, 400);
+ RB.setInputLimits(-400, 400);
+ LF.setInputLimits(-400, 400);
+ LB.setInputLimits(-400, 400);
+
+ // 制御量の最小、最大
+ RF.setOutputLimits(0x48, 0x7D);
+ RB.setOutputLimits(0x48, 0x7D);
+ LF.setOutputLimits(0x48, 0x7D);
+ LB.setOutputLimits(0x48, 0x7D);
+
+ // よくわからんやつ
+ RF.setMode(AUTO_MODE);
+ RB.setMode(AUTO_MODE);
+ LF.setMode(AUTO_MODE);
+ LB.setMode(AUTO_MODE);
+
+ // 目標値
+ RF.setSetPoint(300);
+ RB.setSetPoint(300);
+ LF.setSetPoint(300);
+ LB.setSetPoint(300);
+
+ // センサ出力
+ RF.setProcessValue(rpm_RF);
+ RB.setProcessValue(rpm_RB);
+ LF.setProcessValue(rpm_LF);
+ LB.setProcessValue(rpm_LB);
+
+ // 制御量(計算結果)
+ RF_data[0] = RF.compute();
+ RB_data[0] = RB.compute();
+ LF_data[0] = LF.compute();
+ LB_data[0] = LB.compute();
+
+ // 制御量をPWM値に変換
+ /*
+ true_RF_data[0] = 0x38 - RF_data[0];
+ true_RB_data[0] = 0x38 - RB_data[0];
+ true_LF_data[0] = 0x38 - LF_data[0];
+ true_LB_data[0] = 0x38 - LB_data[0];
+ */
+}
+
int main(void)
{
emergency = 0;
@@ -228,12 +307,66 @@
while(1)
{
+ timer.start();
+ myled = 1;
- PID();
- i2c.write(0xA0, true_RF_data, 1);
- i2c.write(0xA2, true_RB_data, 1);
- i2c.write(0xA4, true_LB_data, 1);
- i2c.write(0xA6, true_LF_data, 1);
+ while(timer.read() <= 5.0f) {
+ //timer.start();
+
+ front_PID();
+ /*
+ ice(0xA0, 0x01);
+ ice(0xA2, 0x02);
+ ice(0xA4, 0x03);
+ ice(0xA6, 0x04);
+ */
+ i2c.write(0xA0, true_RF_data, 1, false);
+ wait_us(20);
+
+ i2c.write(0xA2, true_RB_data, 1, false);
+ wait_us(20);
+
+ i2c.write(0xA4, true_LB_data, 1, false);
+ wait_us(20);
+
+ i2c.write(0xA6, true_LF_data, 1, false);
+ wait_us(20);
+ //wait_ms(50);
+ }
+
+ timer.reset();
+ myled = 0;
+ wait(1);
+
+ RF.reset();
+ RB.reset();
+ LF.reset();
+ LB.reset();
+ wait(1);
+
+ //wait(5);
+
+ /*
+ timer.start();
+ while(timer.read() <= 5.0f) {
+ //timer.start();
+ back_PID();
+ i2c.write(0xA0, RF_data, 1);
+ i2c.write(0xA2, RB_data, 1);
+ i2c.write(0xA4, LB_data, 1);
+ i2c.write(0xA6, LF_data, 1);
+ wait_ms(40);
+ }
+ timer.reset();
+ */
+ /*
+ RF.reset();
+ RB.reset();
+ LF.reset();
+ LB.reset();
+ //wait(5);
+ */
+
/*
// センサ出力値の最小、最大