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@9:bf6d20813364, 2017-09-28 (annotated)

Committer:
Akito914
Date:
Thu Sep 28 10:20:53 2017 +0000
Revision:
9:bf6d20813364
Parent:
8:956a76b98da0
Child:
10:f17b33dd837d
??????????

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