taiyou komazawa
2018 HongoMechaTech A
Diff: main.cpp
- Revision:
- 0:e83b840a5f86
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Sep 18 03:11:01 2018 +0000
@@ -0,0 +1,342 @@
+#include <string.h>
+
+#include "mbed.h"
+#include "Pin.h"
+#include "LinearFunction.h"
+#include "QEI.h"
+#include "MD.h"
+#include "StraightMD.h"
+#include "Timer_PID.h"
+#include "MDPIDSpeed.h"
+
+#define MY_ADDRESS 0xd0
+
+#define M_PI 3.14159265359
+#define DEBUG_ENABLE 0
+#if DEBUG_ENABLE == 1
+#define DEBUG if(DEBUG_ENABLE)
+#define NDEBUG if(!DEBUG_ENABLE)
+#else
+#define DEBUG if(DEBUG_ENABLE)
+#define NDEBUG if(!DEBUG_ENABLE)
+#endif
+#define ROTE_LIMIT ((360.0 * 10) / 360.0)
+#define QEI_MAX_PULSES 512 //エンコーダパルス数(1層分)
+//#define CONTROL_INTERVAL 25 //制御周期
+
+#define DIM_DEG (double)360.00
+
+const double begin_angle = 5.0 / 360.00; //初期角
+double shoot_angle = (325.0 + 9) / 360.00; //発射角 //成功vパラメータ
+double shoot_open_angle = (295.0 + 10 + 10) / 360.00; //成功vパラメータ
+
+double shoot_speed = -210.0; //発射速度[RPM]
+const double speed_limit = 1500;
+
+const double offset_allowable_error = begin_angle * (13.0 / 100.0); //初期角許容誤差率
+const double shoot_allowable_error = shoot_speed * (1.5 / 100.0); //発射速度許容誤差率
+const double shoot_ang_allowable_error = 5.0 / 360.0;
+
+const unsigned int topspeed_time_ms = 300; //射出速度に達するまでの時間
+
+const unsigned int convergence_time_ms = 2000; //初期収束時間
+
+const double offset_pwr_limit = 0.6; //初期角収束時の出力制限
+const double offset_pwr_gain = 0.10; //オフセット時のPWM出力
+
+const double const_moment = 0.027; //角度制御で考慮するモーメントの大きさの係数
+
+
+const bool encorder_rev = true;
+
+I2CSlave slave(pin::sda, pin::scl);
+Ticker I2C_event_trigger;
+Ticker read_angle_timer;
+char I2C_cmd_shoot[4] = "SHT";
+char I2C_cmd_grip[4] = "GRP";
+char I2C_cmd_reset[4] = "RST";
+const int I2C_buffer_size = 16;
+bool I2C_shoot_start = false;
+
+LinearFunction line(1);
+
+InterruptIn on_zero_point(pin::ft_ts);
+
+//DigitalIn button(A3);
+DigitalIn button(PC_13);
+
+DigitalOut dbg_led[3] =
+{ pin::led1, pin::led2, pin::led3 };
+DigitalOut ESV(pin::esv);
+
+PwmOut pwm_out(pin::pwm);
+DigitalOut dir_out(pin::dir);
+
+MD *output;
+QEI *rote;
+
+//------制御動作部分-----------------------
+typedef struct PIDConstract{
+ const double kp;
+ const double ki;
+ const double kd;
+}PID_constract;
+
+//PID_constract PID_const[2] = {{5.6, 2.5, 0.05},{0.00025, 0.00001, 0.000007}};
+PID_constract PID_const[2] = {{5.6, 2.5, 0.05},{0.00020, 0.00001, 0}};
+
+Timer_PID Position_PID(PID_const[0].kp, PID_const[0].ki, PID_const[0].kd);
+
+bool offset_position(bool dir_enable_CW);
+bool set_position(double angle, int time_ms, double allowable_error,
+ double pwr_limit);
+//------------------------------------------
+
+//------腕動作のリセット及び角度測定部分------------
+bool on_offset = false;
+bool rote_reset_enable = false;
+void encorder_angle_reset();
+//-----------------------------------------
+
+//------動作モードトグル部分---------------------
+bool dump_button(bool input, bool reset);
+bool reset = 0;
+//------------------------------------------
+
+void move_ESV(DigitalOut *output, bool close);
+
+double read_angle(bool enable_over_angle, bool dir_rev);
+void on_I2C_event();
+
+void on_read_angle();
+double real_time_angle;
+
+int main()
+{
+ button.mode(PullUp);
+ //slave.address(MY_ADDRESS);
+ //I2C_event_trigger.attach(&on_I2C_event, 100);
+ output = new StraightMD(pin::pwm, pin::dir);
+ rote = new QEI(pin::a_lyr, pin::b_lyr, QEI_MAX_PULSES, 0.005);
+
+ MD_PID_Speed Speed_PID(output, rote, PID_const[1].kp, PID_const[1].ki, PID_const[1].kd);
+
+ on_zero_point.rise(&encorder_angle_reset);
+
+ on_offset = false;
+ while(!offset_position(false)){
+ DEBUG printf("move to offset(init)\tANGLE : %lf\r\n", read_angle(true, encorder_rev) * DIM_DEG);
+ }
+ DEBUG printf("Initial Ready\r\n");
+
+ while(1){
+ bool shot_button = dump_button(!button.read() | I2C_shoot_start, reset);;
+ //on_offset = false;
+
+ if(shot_button){//発射管制モード
+ dbg_led[2].write(false);
+ move_ESV(&ESV, true);
+ wait_ms(500);
+
+ line.reset();
+ Position_PID.reset();
+
+
+ read_angle_timer.attach_us(&on_read_angle, 100);
+ do{
+
+ //line.set(shoot_speed, 0, topspeed_time_ms);
+ //line.set(-0.70 + 0.079, 0, topspeed_time_ms); //成功vパラメータ
+ line.set(-0.70 + 0.0795, 0, topspeed_time_ms);
+ //Speed_PID.drive(line.get_val());
+ //Speed_PID.drive(shoot_speed);
+ output->drive(line.get_val());
+ if(
+ /*read_angle(true, encorder_rev) >= 1.0
+ && read_angle(false, encorder_rev) <= shoot_open_angle
+ && read_angle(false, encorder_rev) >= (shoot_open_angle - shoot_ang_allowable_error))*/
+ read_angle(true, encorder_rev) >= 1.0
+ && real_time_angle <= shoot_open_angle
+ && real_time_angle >= (shoot_open_angle - shoot_ang_allowable_error)){
+ move_ESV(&ESV, false);
+
+ while(/*read_angle(false, encorder_rev)*/real_time_angle <= shoot_angle){
+ //Speed_PID.drive(line.get_val());
+ output->drive(line.get_val());
+ }
+ //wait_ms(250);
+ output->drive(0.00);
+ DEBUG printf("stop!\r\n");
+ //Speed_PID.reset();
+ line.reset();
+ wait_ms(500);
+ break;
+ }
+ //DEBUG printf("SpeedPID TAR : %.1lf_%.1lf RPM : %lf\tPWM : %lf\tF_ANG : %.1lf\tANG : %.3lf\r\n",
+ // shoot_speed + shoot_allowable_error, shoot_speed - shoot_allowable_error, Speed_PID.get_current_rpm(), Speed_PID.get_duty(), read_angle(true, encorder_rev), read_angle(false, encorder_rev));
+ DEBUG printf("%lf \r\n", Speed_PID.get_current_rpm()/*real_time_angle*/);
+ //NDEBUG wait_ms(5);
+ //Speed_PID.read_interval();
+ }while(fabs(read_angle(true, encorder_rev)) < ROTE_LIMIT);
+ read_angle_timer.detach();
+
+ //Speed_PID.reset();
+ line.reset();
+ line.set(0.0, Speed_PID.get_current_rpm(), 1000);
+ do{
+ Speed_PID.drive(line.get_val());
+ DEBUG printf("breaking : %lf\r\n", line.get_val());
+ NDEBUG wait_ms(50);
+ //wait_ms(500);
+ }while(line.get_val() != 0);
+
+ wait_ms(900);
+
+ Speed_PID.reset();
+ I2C_shoot_start = false;
+ on_offset = false;
+ while(!offset_position(false)){
+ DEBUG printf("move to offset\tANGLE : %lf\r\n", read_angle(true, encorder_rev) * DIM_DEG);
+ }
+
+ move_ESV(&ESV, false);
+ line.reset();
+ Position_PID.reset();
+ reset = 1;
+
+ }else{//姿勢管制モード
+ if(!set_position(begin_angle, convergence_time_ms, offset_allowable_error, offset_pwr_limit)){
+ reset = 0;
+ dbg_led[2].write(true);
+ }else{
+ dbg_led[2].write(false);
+ }
+ Position_PID.read_interval();
+ }
+ }
+}
+
+bool dump_button(bool input, bool reset)
+{
+ static bool pushed_flg = false;
+
+ if (input && !pushed_flg)
+ {
+ pushed_flg = true;
+ }
+
+ if (reset)
+ {
+ pushed_flg = false;
+ }
+
+ return pushed_flg;
+}
+
+bool offset_position(bool dir_enable_CW)
+{
+ double pwr = (dir_enable_CW) ? offset_pwr_gain * -2.0 : offset_pwr_gain * 2.0;
+
+ if(fabs(read_angle(true, encorder_rev)) <= 19.0){
+ pwr = (dir_enable_CW) ? offset_pwr_gain * -1 : offset_pwr_gain;
+ rote_reset_enable = true;
+ }
+
+ if(!on_offset){
+ output->drive(pwr);
+ wait_ms(10);
+ return false;
+ }else{
+ rote_reset_enable = false;
+ output->drive(0.00);
+ wait_ms(500);
+ return true;
+ }
+}
+
+bool set_position(double angle, int time_ms, double allowable_error,
+ double power_limit)
+{
+ static double standard_angle = read_angle(true, encorder_rev);
+
+ line.set(angle, standard_angle, time_ms);
+ double duty = -1 * Position_PID.PID(read_angle(true, encorder_rev), line.get_val()) - cos(2 * M_PI * read_angle(true, encorder_rev)) * const_moment;
+ if(duty >= power_limit)
+ duty = power_limit;
+
+ output->drive(duty);
+ //DEBUG printf("position PID TAR : %lf-%lf ANGLE : %lf PWM : %lf\r\n", (line.get_val() + allowable_error) * DIM_DEG, (line.get_val() - allowable_error) * DIM_DEG, read_angle(true, encorder_rev) * DIM_DEG, duty);
+ //DEBUG printf("set_position\r\n");
+ wait_ms(2);
+
+ if(((angle + allowable_error) < read_angle(true, encorder_rev) || (angle - allowable_error) > read_angle(true, encorder_rev))){
+ return true;
+ }else{
+ return false;
+ }
+}
+
+void encorder_angle_reset()
+{
+ if (rote_reset_enable)
+ {
+ printf("reset");
+ rote->reset();
+ on_offset = true;
+ }
+}
+
+void move_ESV(DigitalOut *output, bool close)
+{
+ dbg_led[0].write(close);
+ output->write(close);
+}
+
+double read_angle(bool enable_over_angle, bool dir_rev)
+{
+ double angle = 0;
+ if(enable_over_angle == true)
+ angle = (dir_rev) ? -1 * rote->over_angle() : 1 * rote->over_angle();
+ else
+ angle = (dir_rev) ? 1.00 - rote->angle() : rote->angle();
+ return angle;
+}
+
+void on_read_angle(){
+ real_time_angle = read_angle(false, encorder_rev);
+}
+
+//SHT 角度[deg] 速度[rpm] :発射コマンド
+//SHT 000.0 0000.0 |16文字
+//GRP :掴みコマンド（発射用フラグ）
+//RST :リセットコマンド
+
+void on_I2C_event()
+{
+ char buffer[I2C_buffer_size] = {};
+ switch (slave.receive())
+ {
+ case I2CSlave::ReadAddressed:
+ sprintf(buffer, "%3.1lf %4.1lf %d", shoot_angle, shoot_speed, reset);
+ slave.write(buffer, I2C_buffer_size);
+ break;
+ case I2CSlave::WriteAddressed:
+ slave.read(buffer, I2C_buffer_size);
+ char cmd[4] = {buffer[0], buffer[1], buffer[2], '\0'};
+
+ if(strcmp(cmd, I2C_cmd_shoot) == 0 && ESV.read()){
+ char option_angle[5] = {buffer[4], buffer[5], buffer[6], buffer[7], buffer[8]};
+ char option_speed[6] = {buffer[10], buffer[11], buffer[12], buffer[13], buffer[14], buffer[15]};
+
+ shoot_angle = (360 >= atof(option_angle) && 0 <= atof(option_angle)) ? atof(option_angle) / 360.0 : shoot_angle;
+ shoot_speed = (speed_limit >= atof(option_speed) && 0 <= atof(option_speed)) ? atof(option_speed) : shoot_speed;
+
+ I2C_shoot_start = true;
+ }else if(strcmp(cmd, I2C_cmd_grip) == 0 && !I2C_shoot_start)
+ move_ESV(&ESV, true);
+ else if(strcmp(cmd, I2C_cmd_reset) == 0)
+ NVIC_SystemReset();
+ break;
+ };
+}
+