taiyou komazawa / Mbed 2 deprecated Nucleo_fliping_arm

2018 HongoMechaTech A

Dependencies:   mbed

main.cpp@0:e83b840a5f86, 2018-09-18 (annotated)

Committer:
Komazawa_sun
Date:
Tue Sep 18 03:11:01 2018 +0000
Revision:
0:e83b840a5f86
????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Komazawa_sun 0:e83b840a5f86 1 #include <string.h>
Komazawa_sun 0:e83b840a5f86 2
Komazawa_sun 0:e83b840a5f86 3 #include "mbed.h"
Komazawa_sun 0:e83b840a5f86 4 #include "Pin.h"
Komazawa_sun 0:e83b840a5f86 5 #include "LinearFunction.h"
Komazawa_sun 0:e83b840a5f86 6 #include "QEI.h"
Komazawa_sun 0:e83b840a5f86 7 #include "MD.h"
Komazawa_sun 0:e83b840a5f86 8 #include "StraightMD.h"
Komazawa_sun 0:e83b840a5f86 9 #include "Timer_PID.h"
Komazawa_sun 0:e83b840a5f86 10 #include "MDPIDSpeed.h"
Komazawa_sun 0:e83b840a5f86 11
Komazawa_sun 0:e83b840a5f86 12 #define MY_ADDRESS 0xd0
Komazawa_sun 0:e83b840a5f86 13
Komazawa_sun 0:e83b840a5f86 14 #define M_PI 3.14159265359
Komazawa_sun 0:e83b840a5f86 15 #define DEBUG_ENABLE 0
Komazawa_sun 0:e83b840a5f86 16 #if DEBUG_ENABLE == 1
Komazawa_sun 0:e83b840a5f86 17 #define DEBUG if(DEBUG_ENABLE)
Komazawa_sun 0:e83b840a5f86 18 #define NDEBUG if(!DEBUG_ENABLE)
Komazawa_sun 0:e83b840a5f86 19 #else
Komazawa_sun 0:e83b840a5f86 20 #define DEBUG if(DEBUG_ENABLE)
Komazawa_sun 0:e83b840a5f86 21 #define NDEBUG if(!DEBUG_ENABLE)
Komazawa_sun 0:e83b840a5f86 22 #endif
Komazawa_sun 0:e83b840a5f86 23 #define ROTE_LIMIT ((360.0 * 10) / 360.0)
Komazawa_sun 0:e83b840a5f86 24 #define QEI_MAX_PULSES 512 //エンコーダパルス数(1層分)
Komazawa_sun 0:e83b840a5f86 25 //#define CONTROL_INTERVAL 25 //制御周期
Komazawa_sun 0:e83b840a5f86 26
Komazawa_sun 0:e83b840a5f86 27 #define DIM_DEG (double)360.00
Komazawa_sun 0:e83b840a5f86 28
Komazawa_sun 0:e83b840a5f86 29 const double begin_angle = 5.0 / 360.00; //初期角
Komazawa_sun 0:e83b840a5f86 30 double shoot_angle = (325.0 + 9) / 360.00; //発射角　//成功vパラメータ
Komazawa_sun 0:e83b840a5f86 31 double shoot_open_angle = (295.0 + 10 + 10) / 360.00; //成功vパラメータ
Komazawa_sun 0:e83b840a5f86 32
Komazawa_sun 0:e83b840a5f86 33 double shoot_speed = -210.0; //発射速度[RPM]
Komazawa_sun 0:e83b840a5f86 34 const double speed_limit = 1500;
Komazawa_sun 0:e83b840a5f86 35
Komazawa_sun 0:e83b840a5f86 36 const double offset_allowable_error = begin_angle * (13.0 / 100.0); //初期角許容誤差率
Komazawa_sun 0:e83b840a5f86 37 const double shoot_allowable_error = shoot_speed * (1.5 / 100.0); //発射速度許容誤差率
Komazawa_sun 0:e83b840a5f86 38 const double shoot_ang_allowable_error = 5.0 / 360.0;
Komazawa_sun 0:e83b840a5f86 39
Komazawa_sun 0:e83b840a5f86 40 const unsigned int topspeed_time_ms = 300; //射出速度に達するまでの時間
Komazawa_sun 0:e83b840a5f86 41
Komazawa_sun 0:e83b840a5f86 42 const unsigned int convergence_time_ms = 2000; //初期収束時間
Komazawa_sun 0:e83b840a5f86 43
Komazawa_sun 0:e83b840a5f86 44 const double offset_pwr_limit = 0.6; //初期角収束時の出力制限
Komazawa_sun 0:e83b840a5f86 45 const double offset_pwr_gain = 0.10; //オフセット時のPWM出力
Komazawa_sun 0:e83b840a5f86 46
Komazawa_sun 0:e83b840a5f86 47 const double const_moment = 0.027; //角度制御で考慮するモーメントの大きさの係数
Komazawa_sun 0:e83b840a5f86 48
Komazawa_sun 0:e83b840a5f86 49
Komazawa_sun 0:e83b840a5f86 50 const bool encorder_rev = true;
Komazawa_sun 0:e83b840a5f86 51
Komazawa_sun 0:e83b840a5f86 52 I2CSlave slave(pin::sda, pin::scl);
Komazawa_sun 0:e83b840a5f86 53 Ticker I2C_event_trigger;
Komazawa_sun 0:e83b840a5f86 54 Ticker read_angle_timer;
Komazawa_sun 0:e83b840a5f86 55 char I2C_cmd_shoot[4] = "SHT";
Komazawa_sun 0:e83b840a5f86 56 char I2C_cmd_grip[4] = "GRP";
Komazawa_sun 0:e83b840a5f86 57 char I2C_cmd_reset[4] = "RST";
Komazawa_sun 0:e83b840a5f86 58 const int I2C_buffer_size = 16;
Komazawa_sun 0:e83b840a5f86 59 bool I2C_shoot_start = false;
Komazawa_sun 0:e83b840a5f86 60
Komazawa_sun 0:e83b840a5f86 61 LinearFunction line(1);
Komazawa_sun 0:e83b840a5f86 62
Komazawa_sun 0:e83b840a5f86 63 InterruptIn on_zero_point(pin::ft_ts);
Komazawa_sun 0:e83b840a5f86 64
Komazawa_sun 0:e83b840a5f86 65 //DigitalIn button(A3);
Komazawa_sun 0:e83b840a5f86 66 DigitalIn button(PC_13);
Komazawa_sun 0:e83b840a5f86 67
Komazawa_sun 0:e83b840a5f86 68 DigitalOut dbg_led[3] =
Komazawa_sun 0:e83b840a5f86 69 { pin::led1, pin::led2, pin::led3 };
Komazawa_sun 0:e83b840a5f86 70 DigitalOut ESV(pin::esv);
Komazawa_sun 0:e83b840a5f86 71
Komazawa_sun 0:e83b840a5f86 72 PwmOut pwm_out(pin::pwm);
Komazawa_sun 0:e83b840a5f86 73 DigitalOut dir_out(pin::dir);
Komazawa_sun 0:e83b840a5f86 74
Komazawa_sun 0:e83b840a5f86 75 MD *output;
Komazawa_sun 0:e83b840a5f86 76 QEI *rote;
Komazawa_sun 0:e83b840a5f86 77
Komazawa_sun 0:e83b840a5f86 78 //------制御動作部分-----------------------
Komazawa_sun 0:e83b840a5f86 79 typedef struct PIDConstract{
Komazawa_sun 0:e83b840a5f86 80 const double kp;
Komazawa_sun 0:e83b840a5f86 81 const double ki;
Komazawa_sun 0:e83b840a5f86 82 const double kd;
Komazawa_sun 0:e83b840a5f86 83 }PID_constract;
Komazawa_sun 0:e83b840a5f86 84
Komazawa_sun 0:e83b840a5f86 85 //PID_constract PID_const[2] = {{5.6, 2.5, 0.05},{0.00025, 0.00001, 0.000007}};
Komazawa_sun 0:e83b840a5f86 86 PID_constract PID_const[2] = {{5.6, 2.5, 0.05},{0.00020, 0.00001, 0}};
Komazawa_sun 0:e83b840a5f86 87
Komazawa_sun 0:e83b840a5f86 88 Timer_PID Position_PID(PID_const[0].kp, PID_const[0].ki, PID_const[0].kd);
Komazawa_sun 0:e83b840a5f86 89
Komazawa_sun 0:e83b840a5f86 90 bool offset_position(bool dir_enable_CW);
Komazawa_sun 0:e83b840a5f86 91 bool set_position(double angle, int time_ms, double allowable_error,
Komazawa_sun 0:e83b840a5f86 92 double pwr_limit);
Komazawa_sun 0:e83b840a5f86 93 //------------------------------------------
Komazawa_sun 0:e83b840a5f86 94
Komazawa_sun 0:e83b840a5f86 95 //------腕動作のリセット及び角度測定部分------------
Komazawa_sun 0:e83b840a5f86 96 bool on_offset = false;
Komazawa_sun 0:e83b840a5f86 97 bool rote_reset_enable = false;
Komazawa_sun 0:e83b840a5f86 98 void encorder_angle_reset();
Komazawa_sun 0:e83b840a5f86 99 //-----------------------------------------
Komazawa_sun 0:e83b840a5f86 100
Komazawa_sun 0:e83b840a5f86 101 //------動作モードトグル部分---------------------
Komazawa_sun 0:e83b840a5f86 102 bool dump_button(bool input, bool reset);
Komazawa_sun 0:e83b840a5f86 103 bool reset = 0;
Komazawa_sun 0:e83b840a5f86 104 //------------------------------------------
Komazawa_sun 0:e83b840a5f86 105
Komazawa_sun 0:e83b840a5f86 106 void move_ESV(DigitalOut *output, bool close);
Komazawa_sun 0:e83b840a5f86 107
Komazawa_sun 0:e83b840a5f86 108 double read_angle(bool enable_over_angle, bool dir_rev);
Komazawa_sun 0:e83b840a5f86 109 void on_I2C_event();
Komazawa_sun 0:e83b840a5f86 110
Komazawa_sun 0:e83b840a5f86 111 void on_read_angle();
Komazawa_sun 0:e83b840a5f86 112 double real_time_angle;
Komazawa_sun 0:e83b840a5f86 113
Komazawa_sun 0:e83b840a5f86 114 int main()
Komazawa_sun 0:e83b840a5f86 115 {
Komazawa_sun 0:e83b840a5f86 116 button.mode(PullUp);
Komazawa_sun 0:e83b840a5f86 117 //slave.address(MY_ADDRESS);
Komazawa_sun 0:e83b840a5f86 118 //I2C_event_trigger.attach(&on_I2C_event, 100);
Komazawa_sun 0:e83b840a5f86 119 output = new StraightMD(pin::pwm, pin::dir);
Komazawa_sun 0:e83b840a5f86 120 rote = new QEI(pin::a_lyr, pin::b_lyr, QEI_MAX_PULSES, 0.005);
Komazawa_sun 0:e83b840a5f86 121
Komazawa_sun 0:e83b840a5f86 122 MD_PID_Speed Speed_PID(output, rote, PID_const[1].kp, PID_const[1].ki, PID_const[1].kd);
Komazawa_sun 0:e83b840a5f86 123
Komazawa_sun 0:e83b840a5f86 124 on_zero_point.rise(&encorder_angle_reset);
Komazawa_sun 0:e83b840a5f86 125
Komazawa_sun 0:e83b840a5f86 126 on_offset = false;
Komazawa_sun 0:e83b840a5f86 127 while(!offset_position(false)){
Komazawa_sun 0:e83b840a5f86 128 DEBUG printf("move to offset(init)\tANGLE : %lf\r\n", read_angle(true, encorder_rev) * DIM_DEG);
Komazawa_sun 0:e83b840a5f86 129 }
Komazawa_sun 0:e83b840a5f86 130 DEBUG printf("Initial Ready\r\n");
Komazawa_sun 0:e83b840a5f86 131
Komazawa_sun 0:e83b840a5f86 132 while(1){
Komazawa_sun 0:e83b840a5f86 133 bool shot_button = dump_button(!button.read() | I2C_shoot_start, reset);;
Komazawa_sun 0:e83b840a5f86 134 //on_offset = false;
Komazawa_sun 0:e83b840a5f86 135
Komazawa_sun 0:e83b840a5f86 136 if(shot_button){//発射管制モード
Komazawa_sun 0:e83b840a5f86 137 dbg_led[2].write(false);
Komazawa_sun 0:e83b840a5f86 138 move_ESV(&ESV, true);
Komazawa_sun 0:e83b840a5f86 139 wait_ms(500);
Komazawa_sun 0:e83b840a5f86 140
Komazawa_sun 0:e83b840a5f86 141 line.reset();
Komazawa_sun 0:e83b840a5f86 142 Position_PID.reset();
Komazawa_sun 0:e83b840a5f86 143
Komazawa_sun 0:e83b840a5f86 144
Komazawa_sun 0:e83b840a5f86 145 read_angle_timer.attach_us(&on_read_angle, 100);
Komazawa_sun 0:e83b840a5f86 146 do{
Komazawa_sun 0:e83b840a5f86 147
Komazawa_sun 0:e83b840a5f86 148 //line.set(shoot_speed, 0, topspeed_time_ms);
Komazawa_sun 0:e83b840a5f86 149 //line.set(-0.70 + 0.079, 0, topspeed_time_ms); //成功vパラメータ
Komazawa_sun 0:e83b840a5f86 150 line.set(-0.70 + 0.0795, 0, topspeed_time_ms);
Komazawa_sun 0:e83b840a5f86 151 //Speed_PID.drive(line.get_val());
Komazawa_sun 0:e83b840a5f86 152 //Speed_PID.drive(shoot_speed);
Komazawa_sun 0:e83b840a5f86 153 output->drive(line.get_val());
Komazawa_sun 0:e83b840a5f86 154 if(
Komazawa_sun 0:e83b840a5f86 155 /*read_angle(true, encorder_rev) >= 1.0
Komazawa_sun 0:e83b840a5f86 156 && read_angle(false, encorder_rev) <= shoot_open_angle
Komazawa_sun 0:e83b840a5f86 157 && read_angle(false, encorder_rev) >= (shoot_open_angle - shoot_ang_allowable_error))*/
Komazawa_sun 0:e83b840a5f86 158 read_angle(true, encorder_rev) >= 1.0
Komazawa_sun 0:e83b840a5f86 159 && real_time_angle <= shoot_open_angle
Komazawa_sun 0:e83b840a5f86 160 && real_time_angle >= (shoot_open_angle - shoot_ang_allowable_error)){
Komazawa_sun 0:e83b840a5f86 161 move_ESV(&ESV, false);
Komazawa_sun 0:e83b840a5f86 162
Komazawa_sun 0:e83b840a5f86 163 while(/*read_angle(false, encorder_rev)*/real_time_angle <= shoot_angle){
Komazawa_sun 0:e83b840a5f86 164 //Speed_PID.drive(line.get_val());
Komazawa_sun 0:e83b840a5f86 165 output->drive(line.get_val());
Komazawa_sun 0:e83b840a5f86 166 }
Komazawa_sun 0:e83b840a5f86 167 //wait_ms(250);
Komazawa_sun 0:e83b840a5f86 168 output->drive(0.00);
Komazawa_sun 0:e83b840a5f86 169 DEBUG printf("stop!\r\n");
Komazawa_sun 0:e83b840a5f86 170 //Speed_PID.reset();
Komazawa_sun 0:e83b840a5f86 171 line.reset();
Komazawa_sun 0:e83b840a5f86 172 wait_ms(500);
Komazawa_sun 0:e83b840a5f86 173 break;
Komazawa_sun 0:e83b840a5f86 174 }
Komazawa_sun 0:e83b840a5f86 175 //DEBUG printf("SpeedPID TAR : %.1lf_%.1lf RPM : %lf\tPWM : %lf\tF_ANG : %.1lf\tANG : %.3lf\r\n",
Komazawa_sun 0:e83b840a5f86 176 // 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));
Komazawa_sun 0:e83b840a5f86 177 DEBUG printf("%lf \r\n", Speed_PID.get_current_rpm()/*real_time_angle*/);
Komazawa_sun 0:e83b840a5f86 178 //NDEBUG wait_ms(5);
Komazawa_sun 0:e83b840a5f86 179 //Speed_PID.read_interval();
Komazawa_sun 0:e83b840a5f86 180 }while(fabs(read_angle(true, encorder_rev)) < ROTE_LIMIT);
Komazawa_sun 0:e83b840a5f86 181 read_angle_timer.detach();
Komazawa_sun 0:e83b840a5f86 182
Komazawa_sun 0:e83b840a5f86 183 //Speed_PID.reset();
Komazawa_sun 0:e83b840a5f86 184 line.reset();
Komazawa_sun 0:e83b840a5f86 185 line.set(0.0, Speed_PID.get_current_rpm(), 1000);
Komazawa_sun 0:e83b840a5f86 186 do{
Komazawa_sun 0:e83b840a5f86 187 Speed_PID.drive(line.get_val());
Komazawa_sun 0:e83b840a5f86 188 DEBUG printf("breaking : %lf\r\n", line.get_val());
Komazawa_sun 0:e83b840a5f86 189 NDEBUG wait_ms(50);
Komazawa_sun 0:e83b840a5f86 190 //wait_ms(500);
Komazawa_sun 0:e83b840a5f86 191 }while(line.get_val() != 0);
Komazawa_sun 0:e83b840a5f86 192
Komazawa_sun 0:e83b840a5f86 193 wait_ms(900);
Komazawa_sun 0:e83b840a5f86 194
Komazawa_sun 0:e83b840a5f86 195 Speed_PID.reset();
Komazawa_sun 0:e83b840a5f86 196 I2C_shoot_start = false;
Komazawa_sun 0:e83b840a5f86 197 on_offset = false;
Komazawa_sun 0:e83b840a5f86 198 while(!offset_position(false)){
Komazawa_sun 0:e83b840a5f86 199 DEBUG printf("move to offset\tANGLE : %lf\r\n", read_angle(true, encorder_rev) * DIM_DEG);
Komazawa_sun 0:e83b840a5f86 200 }
Komazawa_sun 0:e83b840a5f86 201
Komazawa_sun 0:e83b840a5f86 202 move_ESV(&ESV, false);
Komazawa_sun 0:e83b840a5f86 203 line.reset();
Komazawa_sun 0:e83b840a5f86 204 Position_PID.reset();
Komazawa_sun 0:e83b840a5f86 205 reset = 1;
Komazawa_sun 0:e83b840a5f86 206
Komazawa_sun 0:e83b840a5f86 207 }else{//姿勢管制モード
Komazawa_sun 0:e83b840a5f86 208 if(!set_position(begin_angle, convergence_time_ms, offset_allowable_error, offset_pwr_limit)){
Komazawa_sun 0:e83b840a5f86 209 reset = 0;
Komazawa_sun 0:e83b840a5f86 210 dbg_led[2].write(true);
Komazawa_sun 0:e83b840a5f86 211 }else{
Komazawa_sun 0:e83b840a5f86 212 dbg_led[2].write(false);
Komazawa_sun 0:e83b840a5f86 213 }
Komazawa_sun 0:e83b840a5f86 214 Position_PID.read_interval();
Komazawa_sun 0:e83b840a5f86 215 }
Komazawa_sun 0:e83b840a5f86 216 }
Komazawa_sun 0:e83b840a5f86 217 }
Komazawa_sun 0:e83b840a5f86 218
Komazawa_sun 0:e83b840a5f86 219 bool dump_button(bool input, bool reset)
Komazawa_sun 0:e83b840a5f86 220 {
Komazawa_sun 0:e83b840a5f86 221 static bool pushed_flg = false;
Komazawa_sun 0:e83b840a5f86 222
Komazawa_sun 0:e83b840a5f86 223 if (input && !pushed_flg)
Komazawa_sun 0:e83b840a5f86 224 {
Komazawa_sun 0:e83b840a5f86 225 pushed_flg = true;
Komazawa_sun 0:e83b840a5f86 226 }
Komazawa_sun 0:e83b840a5f86 227
Komazawa_sun 0:e83b840a5f86 228 if (reset)
Komazawa_sun 0:e83b840a5f86 229 {
Komazawa_sun 0:e83b840a5f86 230 pushed_flg = false;
Komazawa_sun 0:e83b840a5f86 231 }
Komazawa_sun 0:e83b840a5f86 232
Komazawa_sun 0:e83b840a5f86 233 return pushed_flg;
Komazawa_sun 0:e83b840a5f86 234 }
Komazawa_sun 0:e83b840a5f86 235
Komazawa_sun 0:e83b840a5f86 236 bool offset_position(bool dir_enable_CW)
Komazawa_sun 0:e83b840a5f86 237 {
Komazawa_sun 0:e83b840a5f86 238 double pwr = (dir_enable_CW) ? offset_pwr_gain * -2.0 : offset_pwr_gain * 2.0;
Komazawa_sun 0:e83b840a5f86 239
Komazawa_sun 0:e83b840a5f86 240 if(fabs(read_angle(true, encorder_rev)) <= 19.0){
Komazawa_sun 0:e83b840a5f86 241 pwr = (dir_enable_CW) ? offset_pwr_gain * -1 : offset_pwr_gain;
Komazawa_sun 0:e83b840a5f86 242 rote_reset_enable = true;
Komazawa_sun 0:e83b840a5f86 243 }
Komazawa_sun 0:e83b840a5f86 244
Komazawa_sun 0:e83b840a5f86 245 if(!on_offset){
Komazawa_sun 0:e83b840a5f86 246 output->drive(pwr);
Komazawa_sun 0:e83b840a5f86 247 wait_ms(10);
Komazawa_sun 0:e83b840a5f86 248 return false;
Komazawa_sun 0:e83b840a5f86 249 }else{
Komazawa_sun 0:e83b840a5f86 250 rote_reset_enable = false;
Komazawa_sun 0:e83b840a5f86 251 output->drive(0.00);
Komazawa_sun 0:e83b840a5f86 252 wait_ms(500);
Komazawa_sun 0:e83b840a5f86 253 return true;
Komazawa_sun 0:e83b840a5f86 254 }
Komazawa_sun 0:e83b840a5f86 255 }
Komazawa_sun 0:e83b840a5f86 256
Komazawa_sun 0:e83b840a5f86 257 bool set_position(double angle, int time_ms, double allowable_error,
Komazawa_sun 0:e83b840a5f86 258 double power_limit)
Komazawa_sun 0:e83b840a5f86 259 {
Komazawa_sun 0:e83b840a5f86 260 static double standard_angle = read_angle(true, encorder_rev);
Komazawa_sun 0:e83b840a5f86 261
Komazawa_sun 0:e83b840a5f86 262 line.set(angle, standard_angle, time_ms);
Komazawa_sun 0:e83b840a5f86 263 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;
Komazawa_sun 0:e83b840a5f86 264 if(duty >= power_limit)
Komazawa_sun 0:e83b840a5f86 265 duty = power_limit;
Komazawa_sun 0:e83b840a5f86 266
Komazawa_sun 0:e83b840a5f86 267 output->drive(duty);
Komazawa_sun 0:e83b840a5f86 268 //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);
Komazawa_sun 0:e83b840a5f86 269 //DEBUG printf("set_position\r\n");
Komazawa_sun 0:e83b840a5f86 270 wait_ms(2);
Komazawa_sun 0:e83b840a5f86 271
Komazawa_sun 0:e83b840a5f86 272 if(((angle + allowable_error) < read_angle(true, encorder_rev) || (angle - allowable_error) > read_angle(true, encorder_rev))){
Komazawa_sun 0:e83b840a5f86 273 return true;
Komazawa_sun 0:e83b840a5f86 274 }else{
Komazawa_sun 0:e83b840a5f86 275 return false;
Komazawa_sun 0:e83b840a5f86 276 }
Komazawa_sun 0:e83b840a5f86 277 }
Komazawa_sun 0:e83b840a5f86 278
Komazawa_sun 0:e83b840a5f86 279 void encorder_angle_reset()
Komazawa_sun 0:e83b840a5f86 280 {
Komazawa_sun 0:e83b840a5f86 281 if (rote_reset_enable)
Komazawa_sun 0:e83b840a5f86 282 {
Komazawa_sun 0:e83b840a5f86 283 printf("reset");
Komazawa_sun 0:e83b840a5f86 284 rote->reset();
Komazawa_sun 0:e83b840a5f86 285 on_offset = true;
Komazawa_sun 0:e83b840a5f86 286 }
Komazawa_sun 0:e83b840a5f86 287 }
Komazawa_sun 0:e83b840a5f86 288
Komazawa_sun 0:e83b840a5f86 289 void move_ESV(DigitalOut *output, bool close)
Komazawa_sun 0:e83b840a5f86 290 {
Komazawa_sun 0:e83b840a5f86 291 dbg_led[0].write(close);
Komazawa_sun 0:e83b840a5f86 292 output->write(close);
Komazawa_sun 0:e83b840a5f86 293 }
Komazawa_sun 0:e83b840a5f86 294
Komazawa_sun 0:e83b840a5f86 295 double read_angle(bool enable_over_angle, bool dir_rev)
Komazawa_sun 0:e83b840a5f86 296 {
Komazawa_sun 0:e83b840a5f86 297 double angle = 0;
Komazawa_sun 0:e83b840a5f86 298 if(enable_over_angle == true)
Komazawa_sun 0:e83b840a5f86 299 angle = (dir_rev) ? -1 * rote->over_angle() : 1 * rote->over_angle();
Komazawa_sun 0:e83b840a5f86 300 else
Komazawa_sun 0:e83b840a5f86 301 angle = (dir_rev) ? 1.00 - rote->angle() : rote->angle();
Komazawa_sun 0:e83b840a5f86 302 return angle;
Komazawa_sun 0:e83b840a5f86 303 }
Komazawa_sun 0:e83b840a5f86 304
Komazawa_sun 0:e83b840a5f86 305 void on_read_angle(){
Komazawa_sun 0:e83b840a5f86 306 real_time_angle = read_angle(false, encorder_rev);
Komazawa_sun 0:e83b840a5f86 307 }
Komazawa_sun 0:e83b840a5f86 308
Komazawa_sun 0:e83b840a5f86 309 //SHT 角度[deg] 速度[rpm] :発射コマンド
Komazawa_sun 0:e83b840a5f86 310 //SHT 000.0 0000.0 |16文字
Komazawa_sun 0:e83b840a5f86 311 //GRP :掴みコマンド（発射用フラグ）
Komazawa_sun 0:e83b840a5f86 312 //RST :リセットコマンド
Komazawa_sun 0:e83b840a5f86 313
Komazawa_sun 0:e83b840a5f86 314 void on_I2C_event()
Komazawa_sun 0:e83b840a5f86 315 {
Komazawa_sun 0:e83b840a5f86 316 char buffer[I2C_buffer_size] = {};
Komazawa_sun 0:e83b840a5f86 317 switch (slave.receive())
Komazawa_sun 0:e83b840a5f86 318 {
Komazawa_sun 0:e83b840a5f86 319 case I2CSlave::ReadAddressed:
Komazawa_sun 0:e83b840a5f86 320 sprintf(buffer, "%3.1lf %4.1lf %d", shoot_angle, shoot_speed, reset);
Komazawa_sun 0:e83b840a5f86 321 slave.write(buffer, I2C_buffer_size);
Komazawa_sun 0:e83b840a5f86 322 break;
Komazawa_sun 0:e83b840a5f86 323 case I2CSlave::WriteAddressed:
Komazawa_sun 0:e83b840a5f86 324 slave.read(buffer, I2C_buffer_size);
Komazawa_sun 0:e83b840a5f86 325 char cmd[4] = {buffer[0], buffer[1], buffer[2], '\0'};
Komazawa_sun 0:e83b840a5f86 326
Komazawa_sun 0:e83b840a5f86 327 if(strcmp(cmd, I2C_cmd_shoot) == 0 && ESV.read()){
Komazawa_sun 0:e83b840a5f86 328 char option_angle[5] = {buffer[4], buffer[5], buffer[6], buffer[7], buffer[8]};
Komazawa_sun 0:e83b840a5f86 329 char option_speed[6] = {buffer[10], buffer[11], buffer[12], buffer[13], buffer[14], buffer[15]};
Komazawa_sun 0:e83b840a5f86 330
Komazawa_sun 0:e83b840a5f86 331 shoot_angle = (360 >= atof(option_angle) && 0 <= atof(option_angle)) ? atof(option_angle) / 360.0 : shoot_angle;
Komazawa_sun 0:e83b840a5f86 332 shoot_speed = (speed_limit >= atof(option_speed) && 0 <= atof(option_speed)) ? atof(option_speed) : shoot_speed;
Komazawa_sun 0:e83b840a5f86 333
Komazawa_sun 0:e83b840a5f86 334 I2C_shoot_start = true;
Komazawa_sun 0:e83b840a5f86 335 }else if(strcmp(cmd, I2C_cmd_grip) == 0 && !I2C_shoot_start)
Komazawa_sun 0:e83b840a5f86 336 move_ESV(&ESV, true);
Komazawa_sun 0:e83b840a5f86 337 else if(strcmp(cmd, I2C_cmd_reset) == 0)
Komazawa_sun 0:e83b840a5f86 338 NVIC_SystemReset();
Komazawa_sun 0:e83b840a5f86 339 break;
Komazawa_sun 0:e83b840a5f86 340 };
Komazawa_sun 0:e83b840a5f86 341 }
Komazawa_sun 0:e83b840a5f86 342