NITICRobotClub 2018Team-B
自動投射のみです。（手動用にチューニングしました）
Dependencies: HCSR04 PID QEI mbed
Fork of
Lets_move_Seki2018_ver2
by 
INCTRC Information Sharing Test
Diff: main.cpp
- Revision:
- 2:c32991ba628f
- Parent:
- 1:62b321f6c9c3
- Child:
- 3:1223cab367d9
--- a/main.cpp Sun Jul 15 23:56:24 2018 +0000
+++ b/main.cpp Sat Aug 18 04:59:28 2018 +0000
@@ -1,5 +1,5 @@
/* タイマ割り込みを使用して回転数(RPS)を取得し表示するプログラム */
-/* これまでの計算手順では回転数が取得できないことが判明し、改善済である */
+/* 吉田啓人氏が頑張って改善したMDプログラム用に改善しました */
/* 具体的には取得したパルス数を分解能で割り算→掛け算でRPSへ変換から */
/* 取得したパルス数を掛け算→分解能で割ってRPSへ変換としている。 */
/* ロリコンにはTIM2~TIM5がいいらしい(Nucleo_F401re) */
@@ -18,12 +18,13 @@
#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 RF(0.5, 0.4, 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);
+Serial photo(D8, D2);
DigitalOut emergency(PA_13);
DigitalOut myled(D13);
@@ -169,10 +170,11 @@
*/
//pc.printf("RF: %d RB: %d LF: %d LB: %d\n", rpm_RF, rpm_RB, rpm_LF, rpm_LB);
- //pc.printf("%d\n", rpm_RF);
+ pc.printf("%d\n", rpm_LF);
//pc.printf("%lf\n", n_v);
//pc.printf("%lf\n", h_v);
//pc.printf("rpm: %d n_v: %lf\n", rpm[1], n_v);
+ //pc.printf("%lf\n", timer.read());
wheel_RF.reset();
wheel_RB.reset();
@@ -189,20 +191,19 @@
//pc.printf("rpm: %d a_v: %lf\n", rpm[1], a_v);
}
-void front_PID()
+int front_PID(int setpoint)
{
// センサ出力値の最小、最大
- //RF.setInputLimits(0, 1100);
- RF.setInputLimits(-400, 400);
- RB.setInputLimits(-400, 400);
- LF.setInputLimits(-400, 400);
- LB.setInputLimits(-400, 400);
+ RF.setInputLimits(-2000, 2000);
+ RB.setInputLimits(-2000, 2000);
+ LF.setInputLimits(-2000, 2000);
+ LB.setInputLimits(-2000, 2000);
// 制御量の最小、最大
- RF.setOutputLimits(0x01, 0x37);
- RB.setOutputLimits(0x01, 0x37);
- LF.setOutputLimits(0x01, 0x37);
- LB.setOutputLimits(0x01, 0x37);
+ RF.setOutputLimits(0x0C, 0x7C);
+ RB.setOutputLimits(0x0C, 0x7C);
+ LF.setOutputLimits(0x0C, 0x7C);
+ LB.setOutputLimits(0x0C, 0x7C);
// よくわからんやつ
RF.setMode(AUTO_MODE);
@@ -216,10 +217,10 @@
//LB: +
// 目標値
- RF.setSetPoint(300);
- RB.setSetPoint(300);
- LF.setSetPoint(300);
- LB.setSetPoint(300);
+ RF.setSetPoint(setpoint);
+ RB.setSetPoint(setpoint);
+ LF.setSetPoint(setpoint);
+ LB.setSetPoint(setpoint);
// センサ出力
RF.setProcessValue(rpm_RF);
@@ -234,26 +235,27 @@
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];
+ true_RF_data[0] = 0x7D - RF_data[0];
+ true_RB_data[0] = 0x7D - RB_data[0];
+ true_LF_data[0] = 0x7D - LF_data[0];
+ true_LB_data[0] = 0x7D - LB_data[0];
+
+ return 0;
}
-void back_PID()
+int back_PID(int setpoint)
{
// センサ出力値の最小、最大
- //RF.setInputLimits(0, 1100);
- RF.setInputLimits(-400, 400);
- RB.setInputLimits(-400, 400);
- LF.setInputLimits(-400, 400);
- LB.setInputLimits(-400, 400);
+ RF.setInputLimits(0, 2000);
+ RB.setInputLimits(0, 2000);
+ LF.setInputLimits(0, 2000);
+ LB.setInputLimits(0, 2000);
// 制御量の最小、最大
- RF.setOutputLimits(0x48, 0x7D);
- RB.setOutputLimits(0x48, 0x7D);
- LF.setOutputLimits(0x48, 0x7D);
- LB.setOutputLimits(0x48, 0x7D);
+ RF.setOutputLimits(0x84, 0xFF);
+ RB.setOutputLimits(0x84, 0xFF);
+ LF.setOutputLimits(0x84, 0xFF);
+ LB.setOutputLimits(0x84, 0xFF);
// よくわからんやつ
RF.setMode(AUTO_MODE);
@@ -262,16 +264,17 @@
LB.setMode(AUTO_MODE);
// 目標値
- RF.setSetPoint(300);
- RB.setSetPoint(300);
- LF.setSetPoint(300);
- LB.setSetPoint(300);
+ RF.setSetPoint(setpoint);
+ RB.setSetPoint(setpoint);
+ LF.setSetPoint(setpoint);
+ LB.setSetPoint(setpoint);
// センサ出力
- RF.setProcessValue(rpm_RF);
- RB.setProcessValue(rpm_RB);
- LF.setProcessValue(rpm_LF);
- LB.setProcessValue(rpm_LB);
+ //目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
+ 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();
@@ -279,19 +282,13 @@
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];
- */
+ return 0;
}
int main(void)
{
emergency = 0;
- send_data[0] = 0x40;
+ send_data[0] = 0x80;
i2c.write(0xA0, send_data, 1);
i2c.write(0xA2, send_data, 1);
i2c.write(0xA4, send_data, 1);
@@ -307,66 +304,41 @@
while(1)
{
- timer.start();
- myled = 1;
+ /*
+ //後進PID確認
+ back_PID(200);
+ i2c.write(0x20, LF_data, 1, false);
+ wait_us(20);
+ */
+ timer.start();
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);
+ myled = 1;
+ front_PID(200);
+ i2c.write(0x20, 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);
+ while((timer.read() > 5.0f) && (timer.read() <= 10.0f)) {
+ myled = 0;
+ true_LF_data[0] = 0x80;
+ i2c.write(0x20, true_LF_data, 1, false);
+ wait_us(20);
+ }
+ while((timer.read() > 10.0f) && (timer.read() <= 15.0f)) {
+ myled = 1;
+ back_PID(200);
+ i2c.write(0x20, LF_data, 1, false);
+ wait_us(20);
+ }
+ LF.reset();
+ while((timer.read() > 15.0f) && (timer.read() <= 20.0f)) {
+ myled = 0;
+ true_LF_data[0] = 0x80;
+ i2c.write(0x20, true_LF_data, 1, false);
+ wait_us(20);
}
timer.reset();
- */
- /*
- RF.reset();
- RB.reset();
- LF.reset();
- LB.reset();
- //wait(5);
- */
-
/*
// センサ出力値の最小、最大
@@ -413,52 +385,35 @@
*/
/*
- // センサ出力値の最小、最大
- 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]--){
+ //どんどん加速(逆転)
+ for(send_data[0] = 0x81; send_data[0] < 0xF5; 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);
+
+ i2c.write(0x20, send_data, 1);
wait(0.1);
}
-
- //だんだん減速
- for(send_data[0] = 0x02; send_data[0] <= 0x37; send_data[0]++){
+ //だんだん減速(逆転)
+ for(send_data[0] = 0xF4; send_data[0] > 0x80; 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);
+
+ i2c.write(0x20, send_data, 1);
wait(0.1);
}
+ send_data[0] = 0x80;
+ i2c.write(0x20, send_data, 1);
+ wait(5);
*/
}
}