Akito / Mbed 2 deprecated ROBOCON2017_suchan

GifuRobo B term

Dependencies:   FastPWM cal_PID mbed omuni

Fork of omuni_speed_pid by shinji sawai

main.cpp@10:f17b33dd837d, 2017-10-09 (annotated)

Committer:
Akito914
Date:
Mon Oct 09 02:29:36 2017 +0000
Revision:
10:f17b33dd837d
Parent:
9:bf6d20813364 
???????????8?????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sawai 0:6da7d0e457a2 1 #include "mbed.h"
sawai 0:6da7d0e457a2 2 #include "omuni.hpp"
Akito914 1:baab5b88a142 3 #include "cal_PID.hpp"
Akito914 4:5591d3c8a761 4 #include "FastPWM.h"
Akito914 9:bf6d20813364 5 #include "sound_data.h"
sawai 0:6da7d0e457a2 6
Akito914 6:22ad12682a5d 7 #define LIFT_METER_INVERSION 1
Akito914 6:22ad12682a5d 8
Akito914 4:5591d3c8a761 9 const int comTimeout_ms = 200;
Akito914 10:f17b33dd837d 10 const float omuni_speed_max = 4.5f;
Akito914 10:f17b33dd837d 11 const float omuni_burst_coeff = 1.5f;
Akito914 10:f17b33dd837d 12 const float omega_max = 1.5 * 2 * 3.14159265f;
Akito914 10:f17b33dd837d 13 const float omega_f = 1.0 * 2 * 3.14159265f;
Akito914 10:f17b33dd837d 14 const float wrap_radius = 1.5f;
Akito914 10:f17b33dd837d 15 const float wrap_speed = 3.5f;
Akito914 6:22ad12682a5d 16 const int8_t armDuty[] = {-100, -100};
Akito914 1:baab5b88a142 17 const signed int spearDuty = 127;
Akito914 10:f17b33dd837d 18 const int32_t lift_preset_min = 150;
Akito914 10:f17b33dd837d 19 const int32_t lift_preset_max = 410; // + 260
Akito914 3:6223efea43fe 20 const int32_t lift_preset[4] = {
Akito914 3:6223efea43fe 21 lift_preset_min,
Akito914 10:f17b33dd837d 22 (int32_t)((lift_preset_max - lift_preset_min) / 3.0 + lift_preset_min),
Akito914 10:f17b33dd837d 23 (int32_t)((lift_preset_max - lift_preset_min) * 2.0 / 3.0 + lift_preset_min),
Akito914 3:6223efea43fe 24 lift_preset_max,
Akito914 3:6223efea43fe 25 }; // 12bit 0 ~ 4095
Akito914 3:6223efea43fe 26 const int32_t lift_max = lift_preset_max;
Akito914 3:6223efea43fe 27 const int32_t lift_min = lift_preset_min;
Akito914 10:f17b33dd837d 28 const int32_t lift_up_speed = 50;
Akito914 10:f17b33dd837d 29 const int32_t lift_down_speed = 50;
Akito914 9:bf6d20813364 30 const int32_t lift_prescaler = 5;
Akito914 3:6223efea43fe 31
Akito914 1:baab5b88a142 32
Akito914 1:baab5b88a142 33 const int omuniAddr[] = {0x10, 0x12, 0x14}; // 0000 , 1000 , 0100
Akito914 1:baab5b88a142 34 const int armAddr[] = {0x16, 0x18}; // 1100 , 0010
Akito914 1:baab5b88a142 35 const int spearAddr = 0x1a; // 1010
Akito914 1:baab5b88a142 36 const int liftAddr = 0x1c; // 0110
Akito914 4:5591d3c8a761 37 const int EMO_Addr = 0x1e; // 1110
Akito914 4:5591d3c8a761 38
Akito914 4:5591d3c8a761 39 int comTimeout_count = 0;
Akito914 1:baab5b88a142 40
Akito914 7:5cdd805ca8a3 41 bool comTimeout_state = true;
Akito914 1:baab5b88a142 42
Akito914 10:f17b33dd837d 43 volatile bool ems = false;
Akito914 8:956a76b98da0 44 bool recovery = false;
Akito914 1:baab5b88a142 45
sawai 0:6da7d0e457a2 46 bool arm = false;
Akito914 1:baab5b88a142 47 bool spear = false;
Akito914 2:c9de02d6d154 48
Akito914 9:bf6d20813364 49 bool check_beep = false;
Akito914 9:bf6d20813364 50
Akito914 1:baab5b88a142 51 int lift_targ = lift_preset[0];
Akito914 2:c9de02d6d154 52 int lift_stepTarg;
Akito914 3:6223efea43fe 53 bool lift_stepMoving = false;
Akito914 1:baab5b88a142 54 bool first_receive = true;
Akito914 1:baab5b88a142 55 int past_lift_gray = 0b00;
Akito914 1:baab5b88a142 56 int lift_inc = 0;
Akito914 1:baab5b88a142 57
Akito914 9:bf6d20813364 58 int sound_count = 0;
Akito914 4:5591d3c8a761 59
Akito914 6:22ad12682a5d 60 DigitalIn button(USER_BUTTON);
Akito914 1:baab5b88a142 61 DigitalOut led(LED1);
sawai 0:6da7d0e457a2 62
Akito914 4:5591d3c8a761 63 Serial pc(USBTX, USBRX);
Akito914 10:f17b33dd837d 64 Serial comMain(PA_11, PA_12);
Akito914 10:f17b33dd837d 65 Serial comSub(PB_6, PA_10);
Akito914 10:f17b33dd837d 66
Akito914 10:f17b33dd837d 67
sawai 0:6da7d0e457a2 68 I2C i2cMaster(D14, D15);
Akito914 9:bf6d20813364 69 Omuni omuni(&i2cMaster, TIM1, TIM2, TIM3, 533, 2.0f, omuniAddr, 0.4704f, 0.1f);
Akito914 1:baab5b88a142 70 AnalogIn lift_meter(PC_0);
Akito914 1:baab5b88a142 71 DigitalIn spear_sensor(PC_3);
Akito914 1:baab5b88a142 72 Timer mesPeriod;
Akito914 1:baab5b88a142 73 cal_pid lift_pid;
Akito914 4:5591d3c8a761 74 Ticker comTimeout;
Akito914 3:6223efea43fe 75
Akito914 9:bf6d20813364 76 FastPWM soundOut(PB_7);
Akito914 1:baab5b88a142 77
Akito914 1:baab5b88a142 78 float speed_x, speed_y, omega;
Akito914 1:baab5b88a142 79 bool f;
sawai 0:6da7d0e457a2 80
Akito914 1:baab5b88a142 81 typedef union{
Akito914 1:baab5b88a142 82 struct{
Akito914 5:12be2ac0f395 83 unsigned startByte :8;
Akito914 5:12be2ac0f395 84 unsigned joyLX :8;
Akito914 5:12be2ac0f395 85 unsigned joyLY :8;
Akito914 5:12be2ac0f395 86 unsigned joyRX :8;
Akito914 5:12be2ac0f395 87 unsigned joyRY :8;
Akito914 5:12be2ac0f395 88 unsigned L2 :8;
Akito914 5:12be2ac0f395 89 unsigned R2 :8;
Akito914 5:12be2ac0f395 90 unsigned arrowL :1;
Akito914 5:12be2ac0f395 91 unsigned arrowR :1;
Akito914 5:12be2ac0f395 92 unsigned arrowU :1;
Akito914 5:12be2ac0f395 93 unsigned arrowD :1;
Akito914 5:12be2ac0f395 94 unsigned L1 :1;
Akito914 5:12be2ac0f395 95 unsigned R1 :1;
Akito914 5:12be2ac0f395 96 unsigned SELECT :2;
Akito914 5:12be2ac0f395 97 unsigned lift_gray :2;
Akito914 5:12be2ac0f395 98 unsigned spear :1;
Akito914 5:12be2ac0f395 99 unsigned arm :1;
Akito914 5:12be2ac0f395 100 unsigned pushL :1;
Akito914 5:12be2ac0f395 101 unsigned pushR :1;
Akito914 5:12be2ac0f395 102 unsigned EMO :2;
Akito914 5:12be2ac0f395 103 unsigned checksum :8;
Akito914 1:baab5b88a142 104 };
Akito914 5:12be2ac0f395 105
Akito914 5:12be2ac0f395 106 uint8_t list[10];
Akito914 5:12be2ac0f395 107 }comBuf_t;
Akito914 1:baab5b88a142 108
Akito914 5:12be2ac0f395 109 void received_processing(comBuf_t *buf);
Akito914 5:12be2ac0f395 110 int byteSum(int8_t byte);
Akito914 5:12be2ac0f395 111 bool checksum_check(comBuf_t *buf);
Akito914 5:12be2ac0f395 112 int drive_motor(int address,signed int duty);
Akito914 5:12be2ac0f395 113 void emergencyStop();
Akito914 1:baab5b88a142 114
Akito914 9:bf6d20813364 115 /***** sound *****/
Akito914 9:bf6d20813364 116 void pi(int times);
Akito914 9:bf6d20813364 117 void esc_sound(int key);
Akito914 9:bf6d20813364 118 void q_sound();
Akito914 9:bf6d20813364 119 void ans_sound();
Akito914 9:bf6d20813364 120 void piroro();
Akito914 9:bf6d20813364 121 void beep(int T_us,int t_ms);
Akito914 9:bf6d20813364 122 void beep_freq(int freq, int t_ms);
Akito914 9:bf6d20813364 123 void beep_note(int note, int t_ms);
Akito914 9:bf6d20813364 124 void check_beep_turn(bool turn);
Akito914 9:bf6d20813364 125 int getPeriod(int freq);
Akito914 9:bf6d20813364 126 int freq2period_us(int freq);
Akito914 9:bf6d20813364 127 int note2period_us(int note);
Akito914 9:bf6d20813364 128
Akito914 9:bf6d20813364 129 /*****************/
Akito914 9:bf6d20813364 130
Akito914 10:f17b33dd837d 131 comBuf_t comMainBuf;
Akito914 10:f17b33dd837d 132 comBuf_t comSubBuf;
Akito914 10:f17b33dd837d 133
Akito914 5:12be2ac0f395 134
Akito914 8:956a76b98da0 135 void reset(){
Akito914 8:956a76b98da0 136 NVIC_SystemReset();
Akito914 8:956a76b98da0 137 }
Akito914 1:baab5b88a142 138
Akito914 1:baab5b88a142 139 int grayDiffer2bit(int gray, int origin){
Akito914 1:baab5b88a142 140
Akito914 1:baab5b88a142 141 switch(origin){
Akito914 1:baab5b88a142 142 case 0b00:
Akito914 1:baab5b88a142 143 switch(gray){
Akito914 1:baab5b88a142 144 case 0b01: return 1;
Akito914 1:baab5b88a142 145 case 0b10: return -1;
Akito914 1:baab5b88a142 146 default : return 0;
Akito914 1:baab5b88a142 147 }
Akito914 1:baab5b88a142 148 case 0b01:
Akito914 1:baab5b88a142 149 switch(gray){
Akito914 1:baab5b88a142 150 case 0b11: return 1;
Akito914 1:baab5b88a142 151 case 0b00: return -1;
Akito914 1:baab5b88a142 152 default : return 0;
Akito914 1:baab5b88a142 153 }
Akito914 1:baab5b88a142 154 case 0b11:
Akito914 1:baab5b88a142 155 switch(gray){
Akito914 1:baab5b88a142 156 case 0b10: return 1;
Akito914 1:baab5b88a142 157 case 0b01: return -1;
Akito914 1:baab5b88a142 158 default : return 0;
Akito914 1:baab5b88a142 159 }
Akito914 1:baab5b88a142 160 case 0b10:
Akito914 1:baab5b88a142 161 switch(gray){
Akito914 1:baab5b88a142 162 case 0b00: return 1;
Akito914 1:baab5b88a142 163 case 0b11: return -1;
Akito914 1:baab5b88a142 164 default : return 0;
Akito914 1:baab5b88a142 165 }
Akito914 1:baab5b88a142 166 }
Akito914 1:baab5b88a142 167 return 0;
Akito914 1:baab5b88a142 168 }
Akito914 1:baab5b88a142 169
Akito914 5:12be2ac0f395 170 bool checksum_check(comBuf_t *buf){
Akito914 5:12be2ac0f395 171 int sum = 0;
Akito914 5:12be2ac0f395 172
Akito914 5:12be2ac0f395 173 for(int count = 0; count < 9; count++){
Akito914 6:22ad12682a5d 174 sum += buf->list[count];
Akito914 5:12be2ac0f395 175 }
Akito914 6:22ad12682a5d 176 return (sum & 0b01111111) == buf->checksum;
Akito914 5:12be2ac0f395 177 }
Akito914 5:12be2ac0f395 178
sawai 0:6da7d0e457a2 179
Akito914 10:f17b33dd837d 180 void comMain_rx()
sawai 0:6da7d0e457a2 181 {
Akito914 5:12be2ac0f395 182 static int byteCount = 0;
Akito914 10:f17b33dd837d 183 char temp = comMain.getc();
Akito914 5:12be2ac0f395 184
Akito914 5:12be2ac0f395 185 if(temp == 0b10000000){
Akito914 10:f17b33dd837d 186 comMainBuf.list[0] = temp;
Akito914 5:12be2ac0f395 187 byteCount = 1;
Akito914 5:12be2ac0f395 188 }
Akito914 6:22ad12682a5d 189 else if((temp & 0b10000000) != 0){ // 想定外のデータ
Akito914 5:12be2ac0f395 190 byteCount = 0;
Akito914 5:12be2ac0f395 191 }
Akito914 5:12be2ac0f395 192 else if(byteCount > 0){
Akito914 5:12be2ac0f395 193
Akito914 10:f17b33dd837d 194 comMainBuf.list[byteCount] = temp;
Akito914 5:12be2ac0f395 195
Akito914 5:12be2ac0f395 196 if(byteCount < 9)byteCount += 1;
Akito914 5:12be2ac0f395 197 else{ // データは揃った
Akito914 6:22ad12682a5d 198
Akito914 10:f17b33dd837d 199 if(checksum_check(&comMainBuf)){
Akito914 5:12be2ac0f395 200 led = !led;
Akito914 5:12be2ac0f395 201 comTimeout_count = 0;
Akito914 7:5cdd805ca8a3 202 comTimeout_state = false;
Akito914 10:f17b33dd837d 203 received_processing(&comMainBuf);
Akito914 10:f17b33dd837d 204 }
Akito914 10:f17b33dd837d 205 else{
Akito914 10:f17b33dd837d 206 byteCount = 0;
Akito914 10:f17b33dd837d 207 }
Akito914 10:f17b33dd837d 208 }
Akito914 10:f17b33dd837d 209
Akito914 10:f17b33dd837d 210 }
Akito914 10:f17b33dd837d 211
Akito914 10:f17b33dd837d 212 }
Akito914 10:f17b33dd837d 213
Akito914 10:f17b33dd837d 214 void comSub_rx()
Akito914 10:f17b33dd837d 215 {
Akito914 10:f17b33dd837d 216 static int byteCount = 0;
Akito914 10:f17b33dd837d 217 char temp = comSub.getc();
Akito914 10:f17b33dd837d 218
Akito914 10:f17b33dd837d 219 if(temp == 0b10000000){
Akito914 10:f17b33dd837d 220 comSubBuf.list[0] = temp;
Akito914 10:f17b33dd837d 221 byteCount = 1;
Akito914 10:f17b33dd837d 222 }
Akito914 10:f17b33dd837d 223 else if((temp & 0b10000000) != 0){ // 想定外のデータ
Akito914 10:f17b33dd837d 224 byteCount = 0;
Akito914 10:f17b33dd837d 225 }
Akito914 10:f17b33dd837d 226 else if(byteCount > 0){
Akito914 10:f17b33dd837d 227
Akito914 10:f17b33dd837d 228 comSubBuf.list[byteCount] = temp;
Akito914 10:f17b33dd837d 229
Akito914 10:f17b33dd837d 230 if(byteCount < 9)byteCount += 1;
Akito914 10:f17b33dd837d 231 else{ // データは揃った
Akito914 10:f17b33dd837d 232
Akito914 10:f17b33dd837d 233 if(checksum_check(&comSubBuf)){
Akito914 10:f17b33dd837d 234 led = !led;
Akito914 10:f17b33dd837d 235 comTimeout_count = 0;
Akito914 10:f17b33dd837d 236 comTimeout_state = false;
Akito914 10:f17b33dd837d 237 received_processing(&comSubBuf);
Akito914 5:12be2ac0f395 238 }
Akito914 5:12be2ac0f395 239 else{
Akito914 5:12be2ac0f395 240 byteCount = 0;
Akito914 5:12be2ac0f395 241 }
Akito914 5:12be2ac0f395 242 }
Akito914 5:12be2ac0f395 243
Akito914 5:12be2ac0f395 244 }
Akito914 5:12be2ac0f395 245
Akito914 5:12be2ac0f395 246 }
Akito914 5:12be2ac0f395 247
Akito914 5:12be2ac0f395 248 int getJoy7bit(int raw){
Akito914 5:12be2ac0f395 249 return raw - 64;
Akito914 5:12be2ac0f395 250 }
Akito914 5:12be2ac0f395 251
Akito914 5:12be2ac0f395 252 void received_processing(comBuf_t *buf){
Akito914 5:12be2ac0f395 253
Akito914 1:baab5b88a142 254 int lift_nearest_num = 0;
Akito914 10:f17b33dd837d 255 float speed = 0;
Akito914 10:f17b33dd837d 256 float raw_speed_x, raw_speed_y;
Akito914 10:f17b33dd837d 257 float degree;
Akito914 10:f17b33dd837d 258 int angle_num;
Akito914 10:f17b33dd837d 259 const int omuni_angle_split_num = 8;
sawai 0:6da7d0e457a2 260
Akito914 8:956a76b98da0 261 if(buf->EMO != 0){
Akito914 8:956a76b98da0 262 ems = true;
Akito914 8:956a76b98da0 263 }
Akito914 8:956a76b98da0 264 else if(ems == true){
Akito914 8:956a76b98da0 265 reset();
Akito914 8:956a76b98da0 266 }
Akito914 1:baab5b88a142 267
Akito914 9:bf6d20813364 268
Akito914 9:bf6d20813364 269 if(check_beep == false && buf->SELECT == true){
Akito914 9:bf6d20813364 270 check_beep_turn(true);
Akito914 9:bf6d20813364 271 }
Akito914 9:bf6d20813364 272 else if(check_beep == true && buf->SELECT == false){
Akito914 9:bf6d20813364 273 check_beep_turn(false);
Akito914 9:bf6d20813364 274 }
Akito914 9:bf6d20813364 275 check_beep = buf->SELECT;
Akito914 9:bf6d20813364 276
Akito914 10:f17b33dd837d 277 raw_speed_x = -1 * omuni_speed_max * getJoy7bit(buf->joyLX) / 64.0f;
Akito914 10:f17b33dd837d 278 raw_speed_y = -1 * omuni_speed_max * getJoy7bit(buf->joyLY) / 64.0f;
Akito914 10:f17b33dd837d 279
Akito914 10:f17b33dd837d 280 speed = sqrt(raw_speed_x * raw_speed_x + raw_speed_y * raw_speed_y);
Akito914 10:f17b33dd837d 281
Akito914 10:f17b33dd837d 282 degree = atan2((double)raw_speed_y, (double)raw_speed_x) * 180.0 / 3.14159265358979;
Akito914 10:f17b33dd837d 283
Akito914 10:f17b33dd837d 284 angle_num = (double)degree * omuni_angle_split_num / 360.0 + ((degree < 0)?-0.5:0.5);
Akito914 10:f17b33dd837d 285
Akito914 10:f17b33dd837d 286 speed_x = speed * cos(angle_num * 2 * 3.14159265358979 / omuni_angle_split_num);
Akito914 10:f17b33dd837d 287 speed_y = speed * sin(angle_num * 2 * 3.14159265358979 / omuni_angle_split_num);
Akito914 10:f17b33dd837d 288
Akito914 10:f17b33dd837d 289 /*
Akito914 9:bf6d20813364 290 speed_x = -1 * omuni_speed_max * getJoy7bit(buf->joyLX) / 64.0f;
Akito914 9:bf6d20813364 291 speed_y = -1 * omuni_speed_max * getJoy7bit(buf->joyLY) / 64.0f;
Akito914 10:f17b33dd837d 292 */
sawai 0:6da7d0e457a2 293
Akito914 7:5cdd805ca8a3 294 if(buf->pushL == 1){
Akito914 7:5cdd805ca8a3 295 speed_x *= omuni_burst_coeff;
Akito914 7:5cdd805ca8a3 296 speed_y *= omuni_burst_coeff;
Akito914 7:5cdd805ca8a3 297 }
Akito914 7:5cdd805ca8a3 298
Akito914 6:22ad12682a5d 299 if(abs(getJoy7bit(buf->joyRY)) < 20 && abs(getJoy7bit(buf->joyRX)) > 50){
Akito914 5:12be2ac0f395 300 f = 1;
Akito914 5:12be2ac0f395 301 omega = omega_f;
Akito914 9:bf6d20813364 302 if(getJoy7bit(buf->joyRX) > 0)omega *= -1;
Akito914 5:12be2ac0f395 303 }
Akito914 7:5cdd805ca8a3 304 else{
Akito914 9:bf6d20813364 305 int diff = (int)buf->L2 - (int)buf->R2;
Akito914 7:5cdd805ca8a3 306 omega = omega_max * diff / 127.0f;
Akito914 7:5cdd805ca8a3 307 f = 0;
Akito914 7:5cdd805ca8a3 308 }
Akito914 7:5cdd805ca8a3 309
Akito914 5:12be2ac0f395 310 if(buf->R1 || buf->L1){
Akito914 9:bf6d20813364 311 float speed_wrap_x;
Akito914 9:bf6d20813364 312 float speed_wrap_y;
Akito914 9:bf6d20813364 313 float omega_wrap;
Akito914 9:bf6d20813364 314
Akito914 9:bf6d20813364 315 speed_wrap_x = wrap_speed;
Akito914 9:bf6d20813364 316 speed_wrap_y = 0;
Akito914 9:bf6d20813364 317 omega_wrap = wrap_speed / wrap_radius;
Akito914 9:bf6d20813364 318 speed_wrap_x *= buf->R1 ? -1 : 1;
Akito914 9:bf6d20813364 319 omega_wrap *= buf->R1 ? 1 : -1;
Akito914 9:bf6d20813364 320
Akito914 9:bf6d20813364 321 if(f == 1){
Akito914 9:bf6d20813364 322 speed_x = 0;
Akito914 9:bf6d20813364 323 speed_y = 0;
Akito914 9:bf6d20813364 324 omega = 0;
Akito914 9:bf6d20813364 325 }
Akito914 9:bf6d20813364 326 speed_x += speed_wrap_x;
Akito914 9:bf6d20813364 327 speed_y += speed_wrap_y;
Akito914 9:bf6d20813364 328 omega += omega_wrap;
Akito914 5:12be2ac0f395 329 }
Akito914 5:12be2ac0f395 330
Akito914 5:12be2ac0f395 331 arm = buf->arm != 0;
Akito914 5:12be2ac0f395 332
Akito914 5:12be2ac0f395 333 spear = buf->spear != 0;
Akito914 5:12be2ac0f395 334
Akito914 5:12be2ac0f395 335 if(first_receive){
Akito914 5:12be2ac0f395 336 past_lift_gray = buf->lift_gray;
Akito914 5:12be2ac0f395 337 first_receive = false;
Akito914 5:12be2ac0f395 338 }
Akito914 5:12be2ac0f395 339 if(buf->lift_gray != past_lift_gray){ // 段階移動
Akito914 5:12be2ac0f395 340 int lift_currentTarg;
Akito914 5:12be2ac0f395 341 int lift_diff = 0;
Akito914 5:12be2ac0f395 342 int lift_nearest = 10000;
Akito914 5:12be2ac0f395 343 int lift_nextTarg;
Akito914 1:baab5b88a142 344
Akito914 5:12be2ac0f395 345 lift_currentTarg = lift_targ;
Akito914 5:12be2ac0f395 346 if(lift_stepMoving){
Akito914 5:12be2ac0f395 347 lift_nextTarg = lift_stepTarg;
Akito914 5:12be2ac0f395 348 }
Akito914 5:12be2ac0f395 349 else{
Akito914 5:12be2ac0f395 350 lift_nextTarg = lift_currentTarg;
Akito914 1:baab5b88a142 351 }
Akito914 1:baab5b88a142 352
Akito914 5:12be2ac0f395 353 lift_diff = grayDiffer2bit(buf->lift_gray, past_lift_gray);
Akito914 5:12be2ac0f395 354 for(int num = 0; num < 4; num++){ // 最寄りの段階
Akito914 5:12be2ac0f395 355 if(lift_nearest > abs(lift_preset[num] - lift_currentTarg)){
Akito914 5:12be2ac0f395 356 lift_nearest_num = num;
Akito914 5:12be2ac0f395 357 lift_nearest = abs(lift_preset[num] - lift_currentTarg);
Akito914 5:12be2ac0f395 358 }
Akito914 1:baab5b88a142 359 }
Akito914 5:12be2ac0f395 360 if(lift_diff != 0){
Akito914 5:12be2ac0f395 361 if(lift_diff == 1){
Akito914 7:5cdd805ca8a3 362 if(lift_preset[lift_nearest_num] <= lift_currentTarg){
Akito914 5:12be2ac0f395 363 if(lift_nearest_num < 3)lift_nextTarg = lift_preset[lift_nearest_num + 1];
Akito914 5:12be2ac0f395 364 }
Akito914 5:12be2ac0f395 365 else{
Akito914 5:12be2ac0f395 366 lift_nextTarg = lift_preset[lift_nearest_num];
Akito914 1:baab5b88a142 367 }
Akito914 1:baab5b88a142 368 }
Akito914 5:12be2ac0f395 369 else if(lift_diff == -1){
Akito914 7:5cdd805ca8a3 370 if(lift_preset[lift_nearest_num] >= lift_currentTarg){
Akito914 5:12be2ac0f395 371 if(lift_nearest_num > 0)lift_nextTarg = lift_preset[lift_nearest_num - 1];
Akito914 1:baab5b88a142 372 }
Akito914 5:12be2ac0f395 373 else{
Akito914 5:12be2ac0f395 374 lift_nextTarg = lift_preset[lift_nearest_num];
Akito914 1:baab5b88a142 375 }
Akito914 1:baab5b88a142 376 }
Akito914 3:6223efea43fe 377
Akito914 5:12be2ac0f395 378 lift_stepTarg = lift_nextTarg;
Akito914 5:12be2ac0f395 379 lift_stepMoving = true;
Akito914 5:12be2ac0f395 380
Akito914 1:baab5b88a142 381 }
Akito914 1:baab5b88a142 382
Akito914 5:12be2ac0f395 383 }
Akito914 5:12be2ac0f395 384 past_lift_gray = buf->lift_gray;
Akito914 5:12be2ac0f395 385
Akito914 7:5cdd805ca8a3 386
Akito914 6:22ad12682a5d 387 if(abs(getJoy7bit(buf->joyRY)) > 50 && abs(getJoy7bit(buf->joyRX)) < 20){
Akito914 7:5cdd805ca8a3 388 if(getJoy7bit(buf->joyRY) > 0)lift_inc = -5;
Akito914 7:5cdd805ca8a3 389 else lift_inc = 5;
Akito914 5:12be2ac0f395 390 }
Akito914 5:12be2ac0f395 391 else{
Akito914 5:12be2ac0f395 392 lift_inc = 0;
Akito914 5:12be2ac0f395 393 }
Akito914 1:baab5b88a142 394
Akito914 1:baab5b88a142 395 }
Akito914 1:baab5b88a142 396
Akito914 4:5591d3c8a761 397 void comTimeout_intr(){
Akito914 4:5591d3c8a761 398
Akito914 4:5591d3c8a761 399 if(comTimeout_count >= comTimeout_ms){
Akito914 4:5591d3c8a761 400 speed_x = 0;
Akito914 4:5591d3c8a761 401 speed_y = 0;
Akito914 4:5591d3c8a761 402 omega = 0;
Akito914 4:5591d3c8a761 403 arm = 0;
Akito914 4:5591d3c8a761 404 spear = 0;
Akito914 4:5591d3c8a761 405 lift_inc = 0;
Akito914 6:22ad12682a5d 406 comTimeout_state = true;
Akito914 4:5591d3c8a761 407 }
Akito914 4:5591d3c8a761 408 else{
Akito914 4:5591d3c8a761 409 comTimeout_count += 1;
Akito914 4:5591d3c8a761 410 }
Akito914 4:5591d3c8a761 411
Akito914 10:f17b33dd837d 412
Akito914 4:5591d3c8a761 413 }
Akito914 4:5591d3c8a761 414
Akito914 4:5591d3c8a761 415
Akito914 1:baab5b88a142 416 void arm_control(){
Akito914 1:baab5b88a142 417 char armData[2] = {0};
Akito914 1:baab5b88a142 418
Akito914 1:baab5b88a142 419 armData[0] = arm? armDuty[0] : 0 ;
Akito914 1:baab5b88a142 420 armData[1] = arm? armDuty[1] : 0 ;
Akito914 1:baab5b88a142 421
Akito914 1:baab5b88a142 422 i2cMaster.write(armAddr[0], &armData[0], 1);
Akito914 1:baab5b88a142 423 i2cMaster.write(armAddr[1], &armData[1], 1);
Akito914 1:baab5b88a142 424
Akito914 1:baab5b88a142 425 }
Akito914 1:baab5b88a142 426
Akito914 1:baab5b88a142 427 void lift_control(){
Akito914 7:5cdd805ca8a3 428 static int prescaler_count = 0;
Akito914 1:baab5b88a142 429 int32_t meter = 0;
Akito914 1:baab5b88a142 430 signed int duty = 0;
Akito914 3:6223efea43fe 431 int32_t error = 0;
Akito914 1:baab5b88a142 432
Akito914 7:5cdd805ca8a3 433 if(prescaler_count >= lift_prescaler){
Akito914 7:5cdd805ca8a3 434 prescaler_count = 0;
Akito914 7:5cdd805ca8a3 435
Akito914 7:5cdd805ca8a3 436 error = lift_stepTarg - lift_targ;
Akito914 7:5cdd805ca8a3 437 if(lift_stepMoving){
Akito914 7:5cdd805ca8a3 438 if(error > lift_up_speed){
Akito914 7:5cdd805ca8a3 439 lift_targ += lift_up_speed;
Akito914 7:5cdd805ca8a3 440 }
Akito914 7:5cdd805ca8a3 441 else if(error < lift_down_speed * -1){
Akito914 7:5cdd805ca8a3 442 lift_targ -= lift_down_speed;
Akito914 7:5cdd805ca8a3 443 }
Akito914 7:5cdd805ca8a3 444 else{
Akito914 7:5cdd805ca8a3 445 lift_targ = lift_stepTarg;
Akito914 7:5cdd805ca8a3 446 lift_stepMoving = false;
Akito914 7:5cdd805ca8a3 447 }
Akito914 2:c9de02d6d154 448 }
Akito914 2:c9de02d6d154 449 else{
Akito914 7:5cdd805ca8a3 450 if(lift_inc < 0){
Akito914 7:5cdd805ca8a3 451 if(lift_min < lift_targ + lift_inc)lift_targ += lift_inc;
Akito914 7:5cdd805ca8a3 452 }
Akito914 7:5cdd805ca8a3 453 else if(lift_inc > 0){
Akito914 7:5cdd805ca8a3 454 if(lift_max > lift_targ + lift_inc)lift_targ += lift_inc;
Akito914 7:5cdd805ca8a3 455 }
Akito914 2:c9de02d6d154 456 }
sawai 0:6da7d0e457a2 457 }
Akito914 2:c9de02d6d154 458 else{
Akito914 7:5cdd805ca8a3 459 prescaler_count += 1;
sawai 0:6da7d0e457a2 460 }
Akito914 2:c9de02d6d154 461
Akito914 4:5591d3c8a761 462 meter = lift_meter.read_u16() >> 6;
Akito914 6:22ad12682a5d 463 #if LIFT_METER_INVERSION
Akito914 6:22ad12682a5d 464 meter = 1023 - meter;
Akito914 6:22ad12682a5d 465 #endif
Akito914 6:22ad12682a5d 466 duty = lift_pid.get_pid(meter, lift_targ, 1);
Akito914 1:baab5b88a142 467 drive_motor(liftAddr, duty);
Akito914 1:baab5b88a142 468
Akito914 1:baab5b88a142 469 }
Akito914 1:baab5b88a142 470
Akito914 1:baab5b88a142 471 void spear_control(){
Akito914 4:5591d3c8a761 472 const int spear_timeout_short = 100;
Akito914 10:f17b33dd837d 473 const int spear_timeout_long = 200;
Akito914 1:baab5b88a142 474 static int spear_state = 0;
Akito914 4:5591d3c8a761 475 static int spear_timeout_count = 0;
Akito914 1:baab5b88a142 476 signed int duty = 0;
Akito914 1:baab5b88a142 477 int sensor = spear_sensor; // リミット時に1
Akito914 1:baab5b88a142 478
Akito914 1:baab5b88a142 479 switch(spear_state){
Akito914 1:baab5b88a142 480 case 0 : // 待ち
Akito914 1:baab5b88a142 481 if(spear){
Akito914 1:baab5b88a142 482 spear_state = 1;
Akito914 1:baab5b88a142 483 }
Akito914 1:baab5b88a142 484 else{
Akito914 1:baab5b88a142 485 if(sensor == 0){
Akito914 1:baab5b88a142 486 duty = spearDuty * -1;
Akito914 1:baab5b88a142 487 }
Akito914 1:baab5b88a142 488 }
Akito914 1:baab5b88a142 489 break;
Akito914 1:baab5b88a142 490 case 1 : // 初期位置から抜け出す
Akito914 1:baab5b88a142 491 duty = spearDuty;
Akito914 4:5591d3c8a761 492 if(sensor != 0){
Akito914 4:5591d3c8a761 493 if(spear_timeout_count < spear_timeout_short){
Akito914 4:5591d3c8a761 494 spear_timeout_count += 1;
Akito914 4:5591d3c8a761 495 }
Akito914 4:5591d3c8a761 496 else{
Akito914 4:5591d3c8a761 497 spear_timeout_count = 0;
Akito914 4:5591d3c8a761 498 spear_state = 0;
Akito914 4:5591d3c8a761 499 }
Akito914 4:5591d3c8a761 500 }
Akito914 4:5591d3c8a761 501 else{
Akito914 4:5591d3c8a761 502 spear_timeout_count = 0;
Akito914 1:baab5b88a142 503 spear_state = 2;
Akito914 1:baab5b88a142 504 }
Akito914 1:baab5b88a142 505 break;
Akito914 1:baab5b88a142 506 case 2 : // 到達位置まで動かす
Akito914 1:baab5b88a142 507 if(sensor == 0){
Akito914 1:baab5b88a142 508 duty = spearDuty;
Akito914 4:5591d3c8a761 509 if(spear_timeout_count < spear_timeout_long){
Akito914 4:5591d3c8a761 510 spear_timeout_count += 1;
Akito914 4:5591d3c8a761 511 }
Akito914 4:5591d3c8a761 512 else{
Akito914 4:5591d3c8a761 513 spear_timeout_count = 0;
Akito914 4:5591d3c8a761 514 spear_state = 3;
Akito914 4:5591d3c8a761 515 }
Akito914 1:baab5b88a142 516 }
Akito914 1:baab5b88a142 517 else{
Akito914 4:5591d3c8a761 518 spear_timeout_count = 0;
Akito914 1:baab5b88a142 519 spear_state = 3;
Akito914 1:baab5b88a142 520 }
Akito914 1:baab5b88a142 521 break;
Akito914 1:baab5b88a142 522 case 3 : // 到達位置から抜け出す
Akito914 1:baab5b88a142 523 duty = spearDuty * -1;
Akito914 1:baab5b88a142 524 if(sensor == 0){
Akito914 1:baab5b88a142 525 spear_state = 4;
Akito914 1:baab5b88a142 526 }
Akito914 1:baab5b88a142 527 break;
Akito914 1:baab5b88a142 528 case 4 : // 初期位置まで戻る
Akito914 1:baab5b88a142 529 if(sensor == 0){
Akito914 1:baab5b88a142 530 duty = spearDuty * -1;
Akito914 1:baab5b88a142 531 }
Akito914 1:baab5b88a142 532 else{
Akito914 1:baab5b88a142 533 spear_state = 0;
Akito914 1:baab5b88a142 534 }
Akito914 1:baab5b88a142 535 break;
Akito914 1:baab5b88a142 536 default :
Akito914 1:baab5b88a142 537 break;
Akito914 1:baab5b88a142 538 }
Akito914 1:baab5b88a142 539 drive_motor(spearAddr, duty);
Akito914 1:baab5b88a142 540 }
Akito914 1:baab5b88a142 541
Akito914 1:baab5b88a142 542
Akito914 1:baab5b88a142 543 void control(){
Akito914 1:baab5b88a142 544
Akito914 6:22ad12682a5d 545 drive_motor(EMO_Addr, 1);
Akito914 6:22ad12682a5d 546
Akito914 6:22ad12682a5d 547 omuni.set_speed(speed_x, speed_y, omega, f);
Akito914 6:22ad12682a5d 548 omuni.drive();
Akito914 6:22ad12682a5d 549
Akito914 1:baab5b88a142 550 arm_control();
Akito914 1:baab5b88a142 551 lift_control();
Akito914 1:baab5b88a142 552 spear_control();
Akito914 1:baab5b88a142 553
sawai 0:6da7d0e457a2 554 }
sawai 0:6da7d0e457a2 555
sawai 0:6da7d0e457a2 556 int main()
sawai 0:6da7d0e457a2 557 {
Akito914 10:f17b33dd837d 558 bool ex_comTimeout_state = true;
Akito914 10:f17b33dd837d 559 char currentState;
Akito914 10:f17b33dd837d 560
Akito914 4:5591d3c8a761 561 pc.baud(115200);
Akito914 10:f17b33dd837d 562 comMain.baud(19200);
Akito914 10:f17b33dd837d 563 comSub.baud(115200);
Akito914 4:5591d3c8a761 564 pc.printf("Hello!\n");
Akito914 10:f17b33dd837d 565 comMain.attach(comMain_rx, Serial::RxIrq);
Akito914 10:f17b33dd837d 566 comSub.attach(comSub_rx, Serial::RxIrq);
Akito914 1:baab5b88a142 567
Akito914 3:6223efea43fe 568 i2cMaster.frequency(400000);
Akito914 1:baab5b88a142 569
sawai 0:6da7d0e457a2 570 omuni.set_speed(0.0f, 0.0f);
sawai 0:6da7d0e457a2 571 omuni.set_pid(0, 3.0f, 0.07f, 0.05f);
sawai 0:6da7d0e457a2 572 omuni.set_pid(1, 3.0f, 0.07f, 0.05f);
sawai 0:6da7d0e457a2 573 omuni.set_pid(2, 3.0f, 0.07f, 0.05f);
sawai 0:6da7d0e457a2 574
Akito914 10:f17b33dd837d 575 omuni.change_motor_direction(0, false);
Akito914 10:f17b33dd837d 576 omuni.change_motor_direction(1, false);
Akito914 10:f17b33dd837d 577 omuni.change_motor_direction(2, false);
Akito914 10:f17b33dd837d 578 omuni.change_coordinates_direction(true, true);
Akito914 10:f17b33dd837d 579
Akito914 10:f17b33dd837d 580 lift_pid.param(1.0, 0.0, 0.0);
Akito914 1:baab5b88a142 581 lift_pid.period(0.001);
Akito914 1:baab5b88a142 582 lift_pid.output(-127, 127);
Akito914 1:baab5b88a142 583
Akito914 4:5591d3c8a761 584 comTimeout.attach_us(comTimeout_intr, 1000);
Akito914 1:baab5b88a142 585
Akito914 1:baab5b88a142 586 led = 0;
Akito914 1:baab5b88a142 587
Akito914 9:bf6d20813364 588 //esc_sound(0);
Akito914 4:5591d3c8a761 589 /*
Akito914 9:bf6d20813364 590 beep_note(96, 150);
Akito914 9:bf6d20813364 591 beep_note(98, 150);
Akito914 9:bf6d20813364 592 beep_note(100, 220);
Akito914 4:5591d3c8a761 593 */
Akito914 9:bf6d20813364 594 beep_note(100, 500);
Akito914 1:baab5b88a142 595
Akito914 10:f17b33dd837d 596
Akito914 10:f17b33dd837d 597 int initial_meter = lift_meter.read_u16() >> 6;
Akito914 10:f17b33dd837d 598 #if LIFT_METER_INVERSION
Akito914 10:f17b33dd837d 599 initial_meter = 1023 - initial_meter;
Akito914 10:f17b33dd837d 600 #endif
Akito914 10:f17b33dd837d 601
Akito914 10:f17b33dd837d 602 lift_targ = initial_meter;
Akito914 10:f17b33dd837d 603
Akito914 10:f17b33dd837d 604 lift_stepTarg = lift_preset[0];
Akito914 10:f17b33dd837d 605 lift_stepMoving = true;
Akito914 10:f17b33dd837d 606
Akito914 10:f17b33dd837d 607 drive_motor(EMO_Addr, 3);
Akito914 10:f17b33dd837d 608
Akito914 10:f17b33dd837d 609
sawai 0:6da7d0e457a2 610 while(1)
sawai 0:6da7d0e457a2 611 {
Akito914 2:c9de02d6d154 612 wait(0.001);
sawai 0:6da7d0e457a2 613
Akito914 10:f17b33dd837d 614 i2cMaster.read(EMO_Addr, &currentState, 1);
Akito914 10:f17b33dd837d 615 if((currentState & 0b00000010) != 0){
Akito914 10:f17b33dd837d 616 for(int count = 0; count < 15; count++){
Akito914 10:f17b33dd837d 617 beep_note(100, 100);
Akito914 10:f17b33dd837d 618 wait_ms(100);
Akito914 10:f17b33dd837d 619 }
Akito914 10:f17b33dd837d 620 while(ems == false);
Akito914 10:f17b33dd837d 621 emergencyStop();
Akito914 10:f17b33dd837d 622 }
Akito914 10:f17b33dd837d 623
Akito914 6:22ad12682a5d 624 if(comTimeout_state == false){
Akito914 6:22ad12682a5d 625 control();
Akito914 6:22ad12682a5d 626 }
Akito914 6:22ad12682a5d 627 else{
Akito914 6:22ad12682a5d 628 drive_motor(EMO_Addr, 0);
Akito914 6:22ad12682a5d 629 }
Akito914 4:5591d3c8a761 630
Akito914 10:f17b33dd837d 631 if(ex_comTimeout_state == false && comTimeout_state == true){
Akito914 10:f17b33dd837d 632 beep_note(100, 100);
Akito914 10:f17b33dd837d 633 beep_note(96, 100);
Akito914 10:f17b33dd837d 634 }
Akito914 10:f17b33dd837d 635 else if(ex_comTimeout_state == true && comTimeout_state == false){
Akito914 10:f17b33dd837d 636 beep_note(96, 100);
Akito914 10:f17b33dd837d 637 beep_note(100, 100);
Akito914 10:f17b33dd837d 638 }
Akito914 10:f17b33dd837d 639
Akito914 10:f17b33dd837d 640 ex_comTimeout_state = comTimeout_state;
Akito914 10:f17b33dd837d 641
Akito914 1:baab5b88a142 642 if(ems){
Akito914 1:baab5b88a142 643 emergencyStop();
Akito914 1:baab5b88a142 644 }
sawai 0:6da7d0e457a2 645 }
Akito914 1:baab5b88a142 646 }
Akito914 1:baab5b88a142 647
Akito914 1:baab5b88a142 648 int drive_motor(int address,signed int duty){ /* アドレスを指定してモータを駆動 */
Akito914 1:baab5b88a142 649 char send_data;
Akito914 1:baab5b88a142 650 int ack;
Akito914 1:baab5b88a142 651 if((duty>127)||(duty<-128))return -1; /* 範囲外なら送信しない */
Akito914 1:baab5b88a142 652 send_data=duty;
Akito914 1:baab5b88a142 653 ack=i2cMaster.write(address,&send_data,1);
Akito914 1:baab5b88a142 654 return ack;
Akito914 1:baab5b88a142 655 }
Akito914 1:baab5b88a142 656
Akito914 1:baab5b88a142 657 void emergencyStop(){
Akito914 1:baab5b88a142 658 drive_motor(omuniAddr[0], 0);
Akito914 1:baab5b88a142 659 drive_motor(omuniAddr[1], 0);
Akito914 1:baab5b88a142 660 drive_motor(omuniAddr[2], 0);
Akito914 1:baab5b88a142 661 drive_motor(armAddr[0], 0);
Akito914 1:baab5b88a142 662 drive_motor(armAddr[1], 0);
Akito914 1:baab5b88a142 663 drive_motor(liftAddr, 0);
Akito914 1:baab5b88a142 664 drive_motor(spearAddr, 0);
Akito914 4:5591d3c8a761 665
Akito914 4:5591d3c8a761 666 drive_motor(EMO_Addr, 0);
Akito914 4:5591d3c8a761 667
Akito914 10:f17b33dd837d 668 beep_note(100, 100);
Akito914 10:f17b33dd837d 669 beep_note(96, 100);
Akito914 10:f17b33dd837d 670
Akito914 10:f17b33dd837d 671 while(1){
Akito914 10:f17b33dd837d 672 }
Akito914 8:956a76b98da0 673
Akito914 1:baab5b88a142 674 }
Akito914 1:baab5b88a142 675
Akito914 1:baab5b88a142 676
Akito914 9:bf6d20813364 677
Akito914 9:bf6d20813364 678 /*************** sound ***************/
Akito914 9:bf6d20813364 679
Akito914 9:bf6d20813364 680 void pi(int times){
Akito914 9:bf6d20813364 681 int count=0;
Akito914 9:bf6d20813364 682
Akito914 9:bf6d20813364 683 for(count=0;count<times;count++){
Akito914 9:bf6d20813364 684 beep(379,50);
Akito914 9:bf6d20813364 685 wait_ms(50);
Akito914 9:bf6d20813364 686 }
Akito914 9:bf6d20813364 687 wait_ms(300);
Akito914 9:bf6d20813364 688 }
Akito914 9:bf6d20813364 689
Akito914 9:bf6d20813364 690 void esc_sound(int key){
Akito914 9:bf6d20813364 691 int count=0;
Akito914 9:bf6d20813364 692
Akito914 9:bf6d20813364 693 wait_ms(60);
Akito914 9:bf6d20813364 694 beep_note(96 + key,150);
Akito914 9:bf6d20813364 695 beep_note(98 + key,150);
Akito914 9:bf6d20813364 696 beep_note(100 + key,220);
Akito914 9:bf6d20813364 697 wait_ms(1200);
Akito914 9:bf6d20813364 698 for(count=0;count<6;count++){
Akito914 9:bf6d20813364 699 beep_note(96 + key,110);
Akito914 9:bf6d20813364 700 wait_ms(150);
Akito914 9:bf6d20813364 701 }
Akito914 9:bf6d20813364 702 wait_ms(1000);
Akito914 9:bf6d20813364 703 beep_note(96 + key,300);
Akito914 9:bf6d20813364 704 wait_ms(100);
Akito914 9:bf6d20813364 705 }
Akito914 9:bf6d20813364 706
Akito914 9:bf6d20813364 707 void q_sound(void){
Akito914 9:bf6d20813364 708 beep(478,100);
Akito914 9:bf6d20813364 709 beep(379,100);
Akito914 9:bf6d20813364 710 }
Akito914 9:bf6d20813364 711
Akito914 9:bf6d20813364 712 void ans_sound(void){
Akito914 9:bf6d20813364 713 beep(379,100);
Akito914 9:bf6d20813364 714 beep(478,100);
Akito914 9:bf6d20813364 715 }
Akito914 9:bf6d20813364 716
Akito914 9:bf6d20813364 717 void piroro(void){
Akito914 9:bf6d20813364 718 beep(379,100);
Akito914 9:bf6d20813364 719 beep(426,100);
Akito914 9:bf6d20813364 720 beep(478,100);
Akito914 9:bf6d20813364 721 }
Akito914 9:bf6d20813364 722
Akito914 9:bf6d20813364 723 void beep(int T_us,int t_ms){
Akito914 9:bf6d20813364 724
Akito914 9:bf6d20813364 725 if(T_us==0 || t_ms==0)return;
Akito914 9:bf6d20813364 726
Akito914 9:bf6d20813364 727 soundOut.period_us(T_us);
Akito914 9:bf6d20813364 728 soundOut.write(0.50);
Akito914 9:bf6d20813364 729
Akito914 9:bf6d20813364 730 wait_ms(t_ms);
Akito914 9:bf6d20813364 731
Akito914 9:bf6d20813364 732 soundOut.write(0.0);
Akito914 9:bf6d20813364 733 soundOut.period_us(100);
Akito914 9:bf6d20813364 734
Akito914 9:bf6d20813364 735 return;
Akito914 9:bf6d20813364 736 }
Akito914 9:bf6d20813364 737
Akito914 9:bf6d20813364 738 void beep_freq(int freq,int t_ms){
Akito914 9:bf6d20813364 739 beep(1000000.0 / freq, t_ms);
Akito914 9:bf6d20813364 740 }
Akito914 9:bf6d20813364 741
Akito914 9:bf6d20813364 742 void beep_note(int note, int t_ms){
Akito914 9:bf6d20813364 743 beep(pow(2.0, (69 - note) / 12.0) * 1000000.0 / 440.0, t_ms);
Akito914 9:bf6d20813364 744 }
Akito914 9:bf6d20813364 745
Akito914 9:bf6d20813364 746 void check_beep_turn(bool turn){
Akito914 9:bf6d20813364 747 if(turn){
Akito914 9:bf6d20813364 748 soundOut.period_us(note2period_us(note_kouka[sound_count] + kouka_key));
Akito914 9:bf6d20813364 749 soundOut.write(0.50);
Akito914 9:bf6d20813364 750 if(sound_count < kouka_size - 1)sound_count++;
Akito914 9:bf6d20813364 751 else sound_count = 0;
Akito914 9:bf6d20813364 752 }
Akito914 9:bf6d20813364 753 else{
Akito914 9:bf6d20813364 754 soundOut.write(0.0);
Akito914 9:bf6d20813364 755 }
Akito914 9:bf6d20813364 756 }
Akito914 9:bf6d20813364 757
Akito914 9:bf6d20813364 758 int getPeriod_us(int freq){
Akito914 9:bf6d20813364 759 if(freq<=0)return 0;
Akito914 9:bf6d20813364 760 return 1000000.0/freq ;
Akito914 9:bf6d20813364 761 }
Akito914 9:bf6d20813364 762
Akito914 9:bf6d20813364 763
Akito914 9:bf6d20813364 764 int freq2period_us(int freq){
Akito914 9:bf6d20813364 765 if(freq<=0)return 0;
Akito914 9:bf6d20813364 766 return 1000000.0/freq ;
Akito914 9:bf6d20813364 767 }
Akito914 9:bf6d20813364 768
Akito914 9:bf6d20813364 769 int note2period_us(int note){
Akito914 9:bf6d20813364 770 return freq2period_us(440.0 * pow(2.0, (note - 69) / 12.0));
Akito914 9:bf6d20813364 771 }
Akito914 9:bf6d20813364 772
Akito914 9:bf6d20813364 773
Akito914 9:bf6d20813364 774 /*******************************************/
Akito914 9:bf6d20813364 775