NITICRobotClub 2018Team-B
自動投射のみです。（手動用にチューニングしました）
Dependencies: HCSR04 PID QEI mbed
Fork of
Lets_move_Seki2018_ver2
by 
INCTRC Information Sharing Test
Diff: main.cpp
- Revision:
- 0:f73c1b076ae4
- Child:
- 1:62b321f6c9c3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Jul 10 13:40:10 2018 +0000
@@ -0,0 +1,331 @@
+/* タイマ割り込みを使用して回転数(RPS)を取得し表示するプログラム */
+/* これまでの計算手順では回転数が取得できないことが判明し、改善済である */
+/* 具体的には取得したパルス数を分解能で割り算→掛け算でRPSへ変換から */
+/* 取得したパルス数を掛け算→分解能で割ってRPSへ変換としている。 */
+/* ロリコンにはTIM2~TIM5がいいらしい(Nucleo_F401re) */
+/* TIM2(CH1: NC, CH2: D3(PB_3), CH3: D6(PB_10), CH4: NC) */
+/* TIM3(CH1: D5(PB_4), CH2: D9(PC_7), CH3: NC, CH4: NC) */
+/* TIM4(CH1: D10(PB_6), CH2: NC, CH3: NC, CH4: NC) */
+/* TIM5(CH1: NC, CH2: NC, CH3: NC, CH4: NC) */
+/* 2018/6/9追記 */
+/* ついに回転数の！PI制御を実現できました。 */
+/* タイマ割込みで角速度を取得してみようと思います。 */
+
+#include "mbed.h"
+#include "math.h"
+#include "QEI.h"
+#include "PID.h"
+#define PI 3.14159265359
+
+PID controller(0.7, 0.7, 0.0, 0.001);
+PID RF(0.5, 0.3, 0.0, 0.001);
+PID RB(0.5, 0.3, 0.0, 0.001);
+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);
+DigitalOut emergency(PA_13);
+
+/* 以下446基板用 */
+QEI wheel_LB(PA_1, PA_0, NC, 624);
+QEI wheel_RB(PB_6, PB_7, NC, 624);
+QEI wheel_LF(PB_4, PB_5, NC, 624);
+QEI wheel_RF(PC_8, PC_9, NC, 624);
+
+Ticker get_RPS;
+Ticker print_RPS;
+
+int rps_RF;
+int rps_RB;
+int rps_LF;
+int rps_LB;
+
+int rpm_RF;
+int rpm_RB;
+int rpm_LF;
+int rpm_LB;
+
+int pulse_RF;
+int pulse_RB;
+int pulse_LF;
+int pulse_LB;
+
+int counter_RF;
+int counter_RB;
+int counter_LF;
+int counter_LB;
+
+double a_v; /* 角速度 */
+double n_v; /* 速度(秒速) */
+double h_v; /* 速度(時速) */
+double n_a; /* 加速度 */
+
+char send_data[1];
+char true_send_data[1];
+
+char RF_data[1];
+char RB_data[1];
+char LF_data[1];
+char LB_data[1];
+char true_RF_data[1];
+char true_RB_data[1];
+char true_LF_data[1];
+char true_LB_data[1];
+
+/* ロリコン処理関数 */
+void flip();
+/* RPS表示関数 */
+void flip2();
+
+void flip(){
+
+ pulse_RF = wheel_RF.getPulses();
+ pulse_RB = wheel_RB.getPulses();
+ pulse_LF = wheel_LF.getPulses();
+ pulse_LB = wheel_LB.getPulses();
+
+ /* *rps変換 */
+ /*10ms*100 = 1s
+ counter_RB = pulse_RB * 100;
+ counter_LB = pulse_LB * 100;
+ */
+
+ //40ms*25 = 1s
+ counter_RF = pulse_RF * 25;
+ counter_RB = pulse_RB * 25;
+ counter_LF = pulse_LF * 25;
+ counter_LB = pulse_LB * 25;
+
+ /*
+ //100ms*10 = 1s
+ counter_RB = pulse_RB * 10;
+ counter_LB = pulse_LB * 10;
+ */
+
+ /* /分解能 */
+ rps_RF = counter_RF / 100;
+ rps_RB = counter_RB / 100;
+ rps_LF = counter_LF / 100;
+ rps_LB = counter_LB / 100;
+
+ rpm_RF = pulse_RF * 25 * 60 / 100;
+ rpm_RB = pulse_RB * 25 * 60 / 100;
+ rpm_LF = pulse_LF * 25 * 60 / 100;
+ rpm_LB = pulse_LB * 25 * 60 / 100;
+
+ /*
+ rps[0] = counter_RB / 100;
+ rps[1] = counter_LB / 100;
+
+ rpm[0] = pulse_RB * 25 * 60 / 100;
+ rpm[1] = pulse_LB * 25 * 60 / 100;
+ */
+
+ /* RPMから角速度へ変換 */
+ a_v = rpm_LB * PI / 30;
+
+ /* RPMから速度(秒速)へ変換 */
+ /* タイヤ半径を0.05[m]とする */
+ n_v = 0.05 * 2 * PI * rpm_LB / 60;
+
+ /* 速度(秒速)から速度(時速)へ変換 */
+ h_v = n_v * 3600 / 1000;
+
+ //n_a = n_v /
+
+ /*
+ if(rpm[1] < 200){
+ send_data[0]--;
+ }
+ else if(rpm[1] > 250){
+ send_data[1]++;
+ }
+ */
+
+ //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("%lf\n", n_v);
+ //pc.printf("%lf\n", h_v);
+ //pc.printf("rpm: %d n_v: %lf\n", rpm[1], n_v);
+
+ wheel_RF.reset();
+ wheel_RB.reset();
+ wheel_LF.reset();
+ wheel_LB.reset();
+}
+
+void flip2()
+{
+ //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("%lf\n", a_v);
+ //pc.printf("rpm: %d a_v: %lf\n", rpm[1], a_v);
+}
+
+void PID()
+{
+ // センサ出力値の最小、最大
+ //RF.setInputLimits(0, 1100);
+ RF.setInputLimits(-400, 400);
+ RB.setInputLimits(-400, 400);
+ LF.setInputLimits(-400, 400);
+ LB.setInputLimits(-400, 400);
+
+ // 制御量の最小、最大
+ RF.setOutputLimits(0x01, 0x37);
+ RB.setOutputLimits(0x01, 0x37);
+ LF.setOutputLimits(0x01, 0x37);
+ LB.setOutputLimits(0x01, 0x37);
+
+ // よくわからんやつ
+ 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(abs(rpm_RF));
+ RB.setProcessValue(abs(rpm_RB));
+ LF.setProcessValue(abs(rpm_LF));
+ LB.setProcessValue(abs(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;
+ send_data[0] = 0x40;
+ i2c.write(0xA0, send_data, 1);
+ i2c.write(0xA2, send_data, 1);
+ i2c.write(0xA4, send_data, 1);
+ i2c.write(0xA6, send_data, 1);
+ wait(0.1);
+
+ /* 加減速処理を入れた場合処理サイクルの最小周期は40msであった(2018/5/12) */
+ get_RPS.attach_us(&flip, 40000);
+ //get_RPS.attach_us(&flip, 100000);
+
+ //RPS表示(0.1sサイクル)
+ //print_RPS.attach(&flip2, 0.1);
+
+ while(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);
+
+ /*
+ // センサ出力値の最小、最大
+ RF.setInputLimits(0, 1100);
+ RB.setInputLimits(0, 1100);
+ LF.setInputLimits(0, 1100);
+ LB.setInputLimits(0, 1100);
+
+ // 制御量の最小、最大
+ RF.setOutputLimits(0x01, 0x37);
+ RB.setOutputLimits(0x01, 0x37);
+ LF.setOutputLimits(0x01, 0x37);
+ LB.setOutputLimits(0x01, 0x37);
+
+ // よくわからんやつ
+ RF.setMode(AUTO_MODE);
+ RB.setMode(AUTO_MODE);
+ LF.setMode(AUTO_MODE);
+ LB.setMode(AUTO_MODE);
+
+ // 目標値
+ RF.setSetPoint(400);
+ RB.setSetPoint(400);
+ LF.setSetPoint(400);
+ LB.setSetPoint(400);
+
+ // センサ出力
+ RF.setProcessValue(abs(rpm_RF));
+ RB.setProcessValue(abs(rpm_RB));
+ LF.setProcessValue(abs(rpm_LF));
+ LB.setProcessValue(abs(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];
+ */
+
+ /*
+ // センサ出力値の最小、最大
+ controller.setInputLimits(0, 1100);
+
+ // 制御量の最小、最大
+ controller.setOutputLimits(0x01, 0x37);
+
+ // よくわからんやつ
+ controller.setMode(AUTO_MODE);
+
+ // 目標値
+ controller.setSetPoint(400);
+
+ // センサ出力
+ controller.setProcessValue(abs(rpm[1]));
+
+ // 制御量(計算結果)
+ send_data[0] = controller.compute();
+
+ // 制御量をPWM値に変換
+ true_send_data[0] = 0x38 - send_data[0];
+
+ i2c.write(0x88, true_send_data, 1);
+ */
+
+ /*
+ //どんどん加速
+ for(send_data[0] = 0x37; send_data[0] > 0x01; send_data[0]--){
+ //ice(0x88, send_data);
+ //ice(0xA2, send_data);
+ i2c.write(0xA0, send_data, 1);
+ i2c.write(0xA2, send_data, 1);
+ i2c.write(0xA4, send_data, 1);
+ i2c.write(0xA6, send_data, 1);
+ wait(0.1);
+ }
+
+ //だんだん減速
+ for(send_data[0] = 0x02; send_data[0] <= 0x37; send_data[0]++){
+ //ice(0x88, send_data);
+ //ice(0xA2, send_data);
+ i2c.write(0xA0, send_data, 1);
+ i2c.write(0xA2, send_data, 1);
+ i2c.write(0xA4, send_data, 1);
+ i2c.write(0xA6, send_data, 1);
+ wait(0.1);
+ }
+ */
+ }
+}