GifuRobo B term
Dependencies: FastPWM cal_PID mbed omuni
Fork of omuni_speed_pid by
main.cpp@8:956a76b98da0, 2017-09-19 (annotated)
- Committer:
- Akito914
- Date:
- Tue Sep 19 03:19:28 2017 +0000
- Revision:
- 8:956a76b98da0
- Parent:
- 7:5cdd805ca8a3
- Child:
- 9:bf6d20813364
???????????????????????????????????
Who changed what in which revision?
User | Revision | Line number | New 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 | 2:c9de02d6d154 | 4 | #include "timeBaseTrapezoidalMotionCal.h" |
Akito914 | 4:5591d3c8a761 | 5 | #include "FastPWM.h" |
Akito914 | 4:5591d3c8a761 | 6 | #include "soundData.h" |
sawai | 0:6da7d0e457a2 | 7 | |
Akito914 | 6:22ad12682a5d | 8 | #define LIFT_METER_INVERSION 1 |
Akito914 | 6:22ad12682a5d | 9 | |
Akito914 | 4:5591d3c8a761 | 10 | const int comTimeout_ms = 200; |
Akito914 | 4:5591d3c8a761 | 11 | const float omuni_speed[4] = {0, 1.0, 2.0, 3.0}; |
Akito914 | 7:5cdd805ca8a3 | 12 | const float omuni_speed_max = 2.0f; |
Akito914 | 7:5cdd805ca8a3 | 13 | const float omuni_burst_coeff = 1.5f; |
Akito914 | 3:6223efea43fe | 14 | const float omega_max = 0.7 * 2 * 3.14159265f; |
Akito914 | 5:12be2ac0f395 | 15 | const float omega_f = 0.7 * 2 * 3.14159265f; |
Akito914 | 3:6223efea43fe | 16 | const float wrap_radius = 1.0f; |
Akito914 | 4:5591d3c8a761 | 17 | const float wrap_speed = 1.8f; |
Akito914 | 6:22ad12682a5d | 18 | const int8_t armDuty[] = {-100, -100}; |
Akito914 | 1:baab5b88a142 | 19 | const signed int spearDuty = 127; |
Akito914 | 6:22ad12682a5d | 20 | //const int32_t lift_preset_min = 200; |
Akito914 | 6:22ad12682a5d | 21 | const int32_t lift_preset_min = 320; // Temporary use |
Akito914 | 6:22ad12682a5d | 22 | const int32_t lift_preset_max = 480; |
Akito914 | 3:6223efea43fe | 23 | const int32_t lift_preset[4] = { |
Akito914 | 3:6223efea43fe | 24 | lift_preset_min, |
Akito914 | 3:6223efea43fe | 25 | (lift_preset_max - lift_preset_min) / 3.0 + lift_preset_min, |
Akito914 | 3:6223efea43fe | 26 | (lift_preset_max - lift_preset_min) * 2.0 / 3.0 + lift_preset_min, |
Akito914 | 3:6223efea43fe | 27 | lift_preset_max, |
Akito914 | 3:6223efea43fe | 28 | }; // 12bit 0 ~ 4095 |
Akito914 | 3:6223efea43fe | 29 | const int32_t lift_max = lift_preset_max; |
Akito914 | 3:6223efea43fe | 30 | const int32_t lift_min = lift_preset_min; |
Akito914 | 7:5cdd805ca8a3 | 31 | const int32_t lift_up_speed = 10; |
Akito914 | 7:5cdd805ca8a3 | 32 | const int32_t lift_down_speed = 10; |
Akito914 | 7:5cdd805ca8a3 | 33 | const int32_t lift_prescaler = 10; |
Akito914 | 3:6223efea43fe | 34 | |
Akito914 | 1:baab5b88a142 | 35 | |
Akito914 | 1:baab5b88a142 | 36 | const int omuniAddr[] = {0x10, 0x12, 0x14}; // 0000 , 1000 , 0100 |
Akito914 | 1:baab5b88a142 | 37 | const int armAddr[] = {0x16, 0x18}; // 1100 , 0010 |
Akito914 | 1:baab5b88a142 | 38 | const int spearAddr = 0x1a; // 1010 |
Akito914 | 1:baab5b88a142 | 39 | const int liftAddr = 0x1c; // 0110 |
Akito914 | 4:5591d3c8a761 | 40 | const int EMO_Addr = 0x1e; // 1110 |
Akito914 | 4:5591d3c8a761 | 41 | |
Akito914 | 4:5591d3c8a761 | 42 | int comTimeout_count = 0; |
Akito914 | 1:baab5b88a142 | 43 | |
Akito914 | 7:5cdd805ca8a3 | 44 | bool comTimeout_state = true; |
Akito914 | 1:baab5b88a142 | 45 | |
Akito914 | 1:baab5b88a142 | 46 | bool ems = false; |
Akito914 | 8:956a76b98da0 | 47 | bool recovery = false; |
Akito914 | 1:baab5b88a142 | 48 | |
sawai | 0:6da7d0e457a2 | 49 | bool arm = false; |
Akito914 | 1:baab5b88a142 | 50 | bool spear = false; |
Akito914 | 2:c9de02d6d154 | 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 | 4:5591d3c8a761 | 59 | const float soundGain = 1.0; |
Akito914 | 4:5591d3c8a761 | 60 | int soundDataCount = 0; |
Akito914 | 4:5591d3c8a761 | 61 | bool soundEnd = false; |
Akito914 | 4:5591d3c8a761 | 62 | |
Akito914 | 6:22ad12682a5d | 63 | DigitalIn button(USER_BUTTON); |
Akito914 | 1:baab5b88a142 | 64 | DigitalOut led(LED1); |
sawai | 0:6da7d0e457a2 | 65 | |
Akito914 | 4:5591d3c8a761 | 66 | Serial pc(USBTX, USBRX); |
Akito914 | 4:5591d3c8a761 | 67 | Serial com(PA_11, PA_12); |
sawai | 0:6da7d0e457a2 | 68 | I2C i2cMaster(D14, D15); |
Akito914 | 1:baab5b88a142 | 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 | 4:5591d3c8a761 | 76 | FastPWM soundOut(PB_6); |
Akito914 | 4:5591d3c8a761 | 77 | Ticker soundRenew; |
Akito914 | 1:baab5b88a142 | 78 | |
Akito914 | 1:baab5b88a142 | 79 | float speed_x, speed_y, omega; |
Akito914 | 1:baab5b88a142 | 80 | bool f; |
sawai | 0:6da7d0e457a2 | 81 | |
Akito914 | 1:baab5b88a142 | 82 | typedef union{ |
Akito914 | 1:baab5b88a142 | 83 | struct{ |
Akito914 | 5:12be2ac0f395 | 84 | unsigned startByte :8; |
Akito914 | 5:12be2ac0f395 | 85 | unsigned joyLX :8; |
Akito914 | 5:12be2ac0f395 | 86 | unsigned joyLY :8; |
Akito914 | 5:12be2ac0f395 | 87 | unsigned joyRX :8; |
Akito914 | 5:12be2ac0f395 | 88 | unsigned joyRY :8; |
Akito914 | 5:12be2ac0f395 | 89 | unsigned L2 :8; |
Akito914 | 5:12be2ac0f395 | 90 | unsigned R2 :8; |
Akito914 | 5:12be2ac0f395 | 91 | unsigned arrowL :1; |
Akito914 | 5:12be2ac0f395 | 92 | unsigned arrowR :1; |
Akito914 | 5:12be2ac0f395 | 93 | unsigned arrowU :1; |
Akito914 | 5:12be2ac0f395 | 94 | unsigned arrowD :1; |
Akito914 | 5:12be2ac0f395 | 95 | unsigned L1 :1; |
Akito914 | 5:12be2ac0f395 | 96 | unsigned R1 :1; |
Akito914 | 5:12be2ac0f395 | 97 | unsigned SELECT :2; |
Akito914 | 5:12be2ac0f395 | 98 | unsigned lift_gray :2; |
Akito914 | 5:12be2ac0f395 | 99 | unsigned spear :1; |
Akito914 | 5:12be2ac0f395 | 100 | unsigned arm :1; |
Akito914 | 5:12be2ac0f395 | 101 | unsigned pushL :1; |
Akito914 | 5:12be2ac0f395 | 102 | unsigned pushR :1; |
Akito914 | 5:12be2ac0f395 | 103 | unsigned EMO :2; |
Akito914 | 5:12be2ac0f395 | 104 | unsigned checksum :8; |
Akito914 | 1:baab5b88a142 | 105 | }; |
Akito914 | 5:12be2ac0f395 | 106 | |
Akito914 | 5:12be2ac0f395 | 107 | uint8_t list[10]; |
Akito914 | 5:12be2ac0f395 | 108 | }comBuf_t; |
Akito914 | 1:baab5b88a142 | 109 | |
Akito914 | 5:12be2ac0f395 | 110 | void received_processing(comBuf_t *buf); |
Akito914 | 5:12be2ac0f395 | 111 | int byteSum(int8_t byte); |
Akito914 | 5:12be2ac0f395 | 112 | bool checksum_check(comBuf_t *buf); |
Akito914 | 5:12be2ac0f395 | 113 | int drive_motor(int address,signed int duty); |
Akito914 | 5:12be2ac0f395 | 114 | void emergencyStop(); |
Akito914 | 1:baab5b88a142 | 115 | |
Akito914 | 5:12be2ac0f395 | 116 | comBuf_t comBuf; |
Akito914 | 5:12be2ac0f395 | 117 | |
Akito914 | 8:956a76b98da0 | 118 | void reset(){ |
Akito914 | 8:956a76b98da0 | 119 | NVIC_SystemReset(); |
Akito914 | 8:956a76b98da0 | 120 | } |
Akito914 | 1:baab5b88a142 | 121 | |
Akito914 | 1:baab5b88a142 | 122 | int grayDiffer2bit(int gray, int origin){ |
Akito914 | 1:baab5b88a142 | 123 | |
Akito914 | 1:baab5b88a142 | 124 | switch(origin){ |
Akito914 | 1:baab5b88a142 | 125 | case 0b00: |
Akito914 | 1:baab5b88a142 | 126 | switch(gray){ |
Akito914 | 1:baab5b88a142 | 127 | case 0b01: return 1; |
Akito914 | 1:baab5b88a142 | 128 | case 0b10: return -1; |
Akito914 | 1:baab5b88a142 | 129 | default : return 0; |
Akito914 | 1:baab5b88a142 | 130 | } |
Akito914 | 1:baab5b88a142 | 131 | case 0b01: |
Akito914 | 1:baab5b88a142 | 132 | switch(gray){ |
Akito914 | 1:baab5b88a142 | 133 | case 0b11: return 1; |
Akito914 | 1:baab5b88a142 | 134 | case 0b00: return -1; |
Akito914 | 1:baab5b88a142 | 135 | default : return 0; |
Akito914 | 1:baab5b88a142 | 136 | } |
Akito914 | 1:baab5b88a142 | 137 | case 0b11: |
Akito914 | 1:baab5b88a142 | 138 | switch(gray){ |
Akito914 | 1:baab5b88a142 | 139 | case 0b10: return 1; |
Akito914 | 1:baab5b88a142 | 140 | case 0b01: return -1; |
Akito914 | 1:baab5b88a142 | 141 | default : return 0; |
Akito914 | 1:baab5b88a142 | 142 | } |
Akito914 | 1:baab5b88a142 | 143 | case 0b10: |
Akito914 | 1:baab5b88a142 | 144 | switch(gray){ |
Akito914 | 1:baab5b88a142 | 145 | case 0b00: return 1; |
Akito914 | 1:baab5b88a142 | 146 | case 0b11: return -1; |
Akito914 | 1:baab5b88a142 | 147 | default : return 0; |
Akito914 | 1:baab5b88a142 | 148 | } |
Akito914 | 1:baab5b88a142 | 149 | } |
Akito914 | 1:baab5b88a142 | 150 | return 0; |
Akito914 | 1:baab5b88a142 | 151 | } |
Akito914 | 1:baab5b88a142 | 152 | |
Akito914 | 5:12be2ac0f395 | 153 | bool checksum_check(comBuf_t *buf){ |
Akito914 | 5:12be2ac0f395 | 154 | int sum = 0; |
Akito914 | 5:12be2ac0f395 | 155 | |
Akito914 | 5:12be2ac0f395 | 156 | for(int count = 0; count < 9; count++){ |
Akito914 | 6:22ad12682a5d | 157 | sum += buf->list[count]; |
Akito914 | 5:12be2ac0f395 | 158 | } |
Akito914 | 6:22ad12682a5d | 159 | return (sum & 0b01111111) == buf->checksum; |
Akito914 | 5:12be2ac0f395 | 160 | } |
Akito914 | 5:12be2ac0f395 | 161 | |
sawai | 0:6da7d0e457a2 | 162 | |
Akito914 | 4:5591d3c8a761 | 163 | void com_rx() |
sawai | 0:6da7d0e457a2 | 164 | { |
Akito914 | 5:12be2ac0f395 | 165 | static int byteCount = 0; |
Akito914 | 4:5591d3c8a761 | 166 | char temp = com.getc(); |
Akito914 | 5:12be2ac0f395 | 167 | |
Akito914 | 5:12be2ac0f395 | 168 | if(temp == 0b10000000){ |
Akito914 | 5:12be2ac0f395 | 169 | comBuf.list[0] = temp; |
Akito914 | 5:12be2ac0f395 | 170 | byteCount = 1; |
Akito914 | 5:12be2ac0f395 | 171 | } |
Akito914 | 6:22ad12682a5d | 172 | else if((temp & 0b10000000) != 0){ // 想定外のデータ |
Akito914 | 5:12be2ac0f395 | 173 | byteCount = 0; |
Akito914 | 5:12be2ac0f395 | 174 | } |
Akito914 | 5:12be2ac0f395 | 175 | else if(byteCount > 0){ |
Akito914 | 5:12be2ac0f395 | 176 | |
Akito914 | 5:12be2ac0f395 | 177 | comBuf.list[byteCount] = temp; |
Akito914 | 5:12be2ac0f395 | 178 | |
Akito914 | 5:12be2ac0f395 | 179 | if(byteCount < 9)byteCount += 1; |
Akito914 | 5:12be2ac0f395 | 180 | else{ // データは揃った |
Akito914 | 6:22ad12682a5d | 181 | |
Akito914 | 5:12be2ac0f395 | 182 | if(checksum_check(&comBuf)){ |
Akito914 | 5:12be2ac0f395 | 183 | led = !led; |
Akito914 | 5:12be2ac0f395 | 184 | comTimeout_count = 0; |
Akito914 | 7:5cdd805ca8a3 | 185 | comTimeout_state = false; |
Akito914 | 5:12be2ac0f395 | 186 | received_processing(&comBuf); |
Akito914 | 5:12be2ac0f395 | 187 | } |
Akito914 | 5:12be2ac0f395 | 188 | else{ |
Akito914 | 5:12be2ac0f395 | 189 | byteCount = 0; |
Akito914 | 5:12be2ac0f395 | 190 | } |
Akito914 | 5:12be2ac0f395 | 191 | } |
Akito914 | 5:12be2ac0f395 | 192 | |
Akito914 | 5:12be2ac0f395 | 193 | } |
Akito914 | 5:12be2ac0f395 | 194 | |
Akito914 | 5:12be2ac0f395 | 195 | } |
Akito914 | 5:12be2ac0f395 | 196 | |
Akito914 | 5:12be2ac0f395 | 197 | int getJoy7bit(int raw){ |
Akito914 | 5:12be2ac0f395 | 198 | return raw - 64; |
Akito914 | 5:12be2ac0f395 | 199 | } |
Akito914 | 5:12be2ac0f395 | 200 | |
Akito914 | 5:12be2ac0f395 | 201 | void received_processing(comBuf_t *buf){ |
Akito914 | 5:12be2ac0f395 | 202 | |
Akito914 | 1:baab5b88a142 | 203 | int lift_nearest_num = 0; |
sawai | 0:6da7d0e457a2 | 204 | |
Akito914 | 8:956a76b98da0 | 205 | if(buf->EMO != 0){ |
Akito914 | 8:956a76b98da0 | 206 | ems = true; |
Akito914 | 8:956a76b98da0 | 207 | } |
Akito914 | 8:956a76b98da0 | 208 | else if(ems == true){ |
Akito914 | 8:956a76b98da0 | 209 | reset(); |
Akito914 | 8:956a76b98da0 | 210 | } |
Akito914 | 1:baab5b88a142 | 211 | |
Akito914 | 5:12be2ac0f395 | 212 | speed_x = omuni_speed_max * getJoy7bit(buf->joyLX) / 64.0f; |
Akito914 | 5:12be2ac0f395 | 213 | speed_y = omuni_speed_max * getJoy7bit(buf->joyLY) / 64.0f; |
sawai | 0:6da7d0e457a2 | 214 | |
Akito914 | 7:5cdd805ca8a3 | 215 | if(buf->pushL == 1){ |
Akito914 | 7:5cdd805ca8a3 | 216 | speed_x *= omuni_burst_coeff; |
Akito914 | 7:5cdd805ca8a3 | 217 | speed_y *= omuni_burst_coeff; |
Akito914 | 7:5cdd805ca8a3 | 218 | } |
Akito914 | 7:5cdd805ca8a3 | 219 | |
Akito914 | 6:22ad12682a5d | 220 | if(abs(getJoy7bit(buf->joyRY)) < 20 && abs(getJoy7bit(buf->joyRX)) > 50){ |
Akito914 | 5:12be2ac0f395 | 221 | f = 1; |
Akito914 | 5:12be2ac0f395 | 222 | omega = omega_f; |
Akito914 | 6:22ad12682a5d | 223 | if(getJoy7bit(buf->joyRX) < 0)omega *= -1; |
Akito914 | 5:12be2ac0f395 | 224 | } |
Akito914 | 7:5cdd805ca8a3 | 225 | else{ |
Akito914 | 7:5cdd805ca8a3 | 226 | int diff = (int)buf->R2 - (int)buf->L2; |
Akito914 | 7:5cdd805ca8a3 | 227 | omega = omega_max * diff / 127.0f; |
Akito914 | 7:5cdd805ca8a3 | 228 | f = 0; |
Akito914 | 7:5cdd805ca8a3 | 229 | } |
Akito914 | 7:5cdd805ca8a3 | 230 | |
Akito914 | 7:5cdd805ca8a3 | 231 | /* |
Akito914 | 6:22ad12682a5d | 232 | else if(buf->L2 >= 5 && buf->R2 < 5){ |
Akito914 | 6:22ad12682a5d | 233 | omega = omega_max * buf->L2 * -1.0f / 127.0f; |
Akito914 | 5:12be2ac0f395 | 234 | f = 0; |
Akito914 | 5:12be2ac0f395 | 235 | } |
Akito914 | 5:12be2ac0f395 | 236 | else if(buf->L2 < 5 && buf->R2 >= 5){ |
Akito914 | 6:22ad12682a5d | 237 | omega = omega_max * buf->R2 * 1.0f / 127.0f; |
Akito914 | 6:22ad12682a5d | 238 | f = 0; |
Akito914 | 6:22ad12682a5d | 239 | } |
Akito914 | 6:22ad12682a5d | 240 | else{ |
Akito914 | 6:22ad12682a5d | 241 | omega = 0; |
Akito914 | 5:12be2ac0f395 | 242 | f = 0; |
sawai | 0:6da7d0e457a2 | 243 | } |
Akito914 | 7:5cdd805ca8a3 | 244 | */ |
sawai | 0:6da7d0e457a2 | 245 | |
Akito914 | 5:12be2ac0f395 | 246 | if(buf->R1 || buf->L1){ |
Akito914 | 5:12be2ac0f395 | 247 | speed_x = wrap_speed; |
Akito914 | 5:12be2ac0f395 | 248 | speed_y = 0; |
Akito914 | 5:12be2ac0f395 | 249 | f = 0; |
Akito914 | 5:12be2ac0f395 | 250 | omega = wrap_speed / wrap_radius; |
Akito914 | 5:12be2ac0f395 | 251 | speed_x *= buf->L1 ? 1 : -1; |
Akito914 | 5:12be2ac0f395 | 252 | omega *= buf->L1 ? 1 : -1; |
Akito914 | 5:12be2ac0f395 | 253 | } |
Akito914 | 5:12be2ac0f395 | 254 | |
Akito914 | 5:12be2ac0f395 | 255 | arm = buf->arm != 0; |
Akito914 | 5:12be2ac0f395 | 256 | |
Akito914 | 5:12be2ac0f395 | 257 | spear = buf->spear != 0; |
Akito914 | 5:12be2ac0f395 | 258 | |
Akito914 | 5:12be2ac0f395 | 259 | if(first_receive){ |
Akito914 | 5:12be2ac0f395 | 260 | past_lift_gray = buf->lift_gray; |
Akito914 | 5:12be2ac0f395 | 261 | first_receive = false; |
Akito914 | 5:12be2ac0f395 | 262 | } |
Akito914 | 5:12be2ac0f395 | 263 | if(buf->lift_gray != past_lift_gray){ // 段階移動 |
Akito914 | 5:12be2ac0f395 | 264 | int lift_currentTarg; |
Akito914 | 5:12be2ac0f395 | 265 | int lift_diff = 0; |
Akito914 | 5:12be2ac0f395 | 266 | int lift_nearest = 10000; |
Akito914 | 5:12be2ac0f395 | 267 | int lift_nextTarg; |
Akito914 | 1:baab5b88a142 | 268 | |
Akito914 | 5:12be2ac0f395 | 269 | lift_currentTarg = lift_targ; |
Akito914 | 5:12be2ac0f395 | 270 | if(lift_stepMoving){ |
Akito914 | 5:12be2ac0f395 | 271 | lift_nextTarg = lift_stepTarg; |
Akito914 | 5:12be2ac0f395 | 272 | } |
Akito914 | 5:12be2ac0f395 | 273 | else{ |
Akito914 | 5:12be2ac0f395 | 274 | lift_nextTarg = lift_currentTarg; |
Akito914 | 1:baab5b88a142 | 275 | } |
Akito914 | 1:baab5b88a142 | 276 | |
Akito914 | 5:12be2ac0f395 | 277 | lift_diff = grayDiffer2bit(buf->lift_gray, past_lift_gray); |
Akito914 | 5:12be2ac0f395 | 278 | for(int num = 0; num < 4; num++){ // 最寄りの段階 |
Akito914 | 5:12be2ac0f395 | 279 | if(lift_nearest > abs(lift_preset[num] - lift_currentTarg)){ |
Akito914 | 5:12be2ac0f395 | 280 | lift_nearest_num = num; |
Akito914 | 5:12be2ac0f395 | 281 | lift_nearest = abs(lift_preset[num] - lift_currentTarg); |
Akito914 | 5:12be2ac0f395 | 282 | } |
Akito914 | 1:baab5b88a142 | 283 | } |
Akito914 | 5:12be2ac0f395 | 284 | if(lift_diff != 0){ |
Akito914 | 5:12be2ac0f395 | 285 | if(lift_diff == 1){ |
Akito914 | 7:5cdd805ca8a3 | 286 | if(lift_preset[lift_nearest_num] <= lift_currentTarg){ |
Akito914 | 5:12be2ac0f395 | 287 | if(lift_nearest_num < 3)lift_nextTarg = lift_preset[lift_nearest_num + 1]; |
Akito914 | 5:12be2ac0f395 | 288 | } |
Akito914 | 5:12be2ac0f395 | 289 | else{ |
Akito914 | 5:12be2ac0f395 | 290 | lift_nextTarg = lift_preset[lift_nearest_num]; |
Akito914 | 1:baab5b88a142 | 291 | } |
Akito914 | 1:baab5b88a142 | 292 | } |
Akito914 | 5:12be2ac0f395 | 293 | else if(lift_diff == -1){ |
Akito914 | 7:5cdd805ca8a3 | 294 | if(lift_preset[lift_nearest_num] >= lift_currentTarg){ |
Akito914 | 5:12be2ac0f395 | 295 | if(lift_nearest_num > 0)lift_nextTarg = lift_preset[lift_nearest_num - 1]; |
Akito914 | 1:baab5b88a142 | 296 | } |
Akito914 | 5:12be2ac0f395 | 297 | else{ |
Akito914 | 5:12be2ac0f395 | 298 | lift_nextTarg = lift_preset[lift_nearest_num]; |
Akito914 | 1:baab5b88a142 | 299 | } |
Akito914 | 1:baab5b88a142 | 300 | } |
Akito914 | 3:6223efea43fe | 301 | |
Akito914 | 5:12be2ac0f395 | 302 | lift_stepTarg = lift_nextTarg; |
Akito914 | 5:12be2ac0f395 | 303 | lift_stepMoving = true; |
Akito914 | 5:12be2ac0f395 | 304 | |
Akito914 | 1:baab5b88a142 | 305 | } |
Akito914 | 1:baab5b88a142 | 306 | |
Akito914 | 5:12be2ac0f395 | 307 | } |
Akito914 | 5:12be2ac0f395 | 308 | past_lift_gray = buf->lift_gray; |
Akito914 | 5:12be2ac0f395 | 309 | |
Akito914 | 7:5cdd805ca8a3 | 310 | |
Akito914 | 6:22ad12682a5d | 311 | if(abs(getJoy7bit(buf->joyRY)) > 50 && abs(getJoy7bit(buf->joyRX)) < 20){ |
Akito914 | 7:5cdd805ca8a3 | 312 | if(getJoy7bit(buf->joyRY) > 0)lift_inc = -5; |
Akito914 | 7:5cdd805ca8a3 | 313 | else lift_inc = 5; |
Akito914 | 5:12be2ac0f395 | 314 | } |
Akito914 | 5:12be2ac0f395 | 315 | else{ |
Akito914 | 5:12be2ac0f395 | 316 | lift_inc = 0; |
Akito914 | 5:12be2ac0f395 | 317 | } |
Akito914 | 1:baab5b88a142 | 318 | |
Akito914 | 1:baab5b88a142 | 319 | } |
Akito914 | 1:baab5b88a142 | 320 | |
Akito914 | 1:baab5b88a142 | 321 | |
Akito914 | 4:5591d3c8a761 | 322 | void soundRenew_intr(){ |
Akito914 | 4:5591d3c8a761 | 323 | |
Akito914 | 4:5591d3c8a761 | 324 | if(soundDataCount < soundData_size - 1000){ |
Akito914 | 4:5591d3c8a761 | 325 | |
Akito914 | 4:5591d3c8a761 | 326 | soundOut.write((soundData[soundDataCount] * soundGain / 256.0) + 0.5); |
Akito914 | 4:5591d3c8a761 | 327 | |
Akito914 | 4:5591d3c8a761 | 328 | soundDataCount++; |
Akito914 | 4:5591d3c8a761 | 329 | |
Akito914 | 4:5591d3c8a761 | 330 | } |
Akito914 | 4:5591d3c8a761 | 331 | else{ |
Akito914 | 4:5591d3c8a761 | 332 | soundEnd = true; |
Akito914 | 4:5591d3c8a761 | 333 | } |
Akito914 | 4:5591d3c8a761 | 334 | } |
Akito914 | 4:5591d3c8a761 | 335 | |
Akito914 | 4:5591d3c8a761 | 336 | void comTimeout_intr(){ |
Akito914 | 4:5591d3c8a761 | 337 | |
Akito914 | 4:5591d3c8a761 | 338 | if(comTimeout_count >= comTimeout_ms){ |
Akito914 | 4:5591d3c8a761 | 339 | speed_x = 0; |
Akito914 | 4:5591d3c8a761 | 340 | speed_y = 0; |
Akito914 | 4:5591d3c8a761 | 341 | omega = 0; |
Akito914 | 4:5591d3c8a761 | 342 | arm = 0; |
Akito914 | 4:5591d3c8a761 | 343 | spear = 0; |
Akito914 | 4:5591d3c8a761 | 344 | lift_inc = 0; |
Akito914 | 6:22ad12682a5d | 345 | comTimeout_state = true; |
Akito914 | 4:5591d3c8a761 | 346 | } |
Akito914 | 4:5591d3c8a761 | 347 | else{ |
Akito914 | 4:5591d3c8a761 | 348 | comTimeout_count += 1; |
Akito914 | 4:5591d3c8a761 | 349 | } |
Akito914 | 4:5591d3c8a761 | 350 | |
Akito914 | 4:5591d3c8a761 | 351 | } |
Akito914 | 4:5591d3c8a761 | 352 | |
Akito914 | 4:5591d3c8a761 | 353 | |
Akito914 | 1:baab5b88a142 | 354 | void arm_control(){ |
Akito914 | 1:baab5b88a142 | 355 | char armData[2] = {0}; |
Akito914 | 1:baab5b88a142 | 356 | |
Akito914 | 1:baab5b88a142 | 357 | armData[0] = arm? armDuty[0] : 0 ; |
Akito914 | 1:baab5b88a142 | 358 | armData[1] = arm? armDuty[1] : 0 ; |
Akito914 | 1:baab5b88a142 | 359 | |
Akito914 | 1:baab5b88a142 | 360 | i2cMaster.write(armAddr[0], &armData[0], 1); |
Akito914 | 1:baab5b88a142 | 361 | i2cMaster.write(armAddr[1], &armData[1], 1); |
Akito914 | 1:baab5b88a142 | 362 | |
Akito914 | 1:baab5b88a142 | 363 | } |
Akito914 | 1:baab5b88a142 | 364 | |
Akito914 | 1:baab5b88a142 | 365 | void lift_control(){ |
Akito914 | 7:5cdd805ca8a3 | 366 | static int prescaler_count = 0; |
Akito914 | 1:baab5b88a142 | 367 | int32_t meter = 0; |
Akito914 | 1:baab5b88a142 | 368 | signed int duty = 0; |
Akito914 | 3:6223efea43fe | 369 | int32_t error = 0; |
Akito914 | 1:baab5b88a142 | 370 | |
Akito914 | 7:5cdd805ca8a3 | 371 | if(prescaler_count >= lift_prescaler){ |
Akito914 | 7:5cdd805ca8a3 | 372 | prescaler_count = 0; |
Akito914 | 7:5cdd805ca8a3 | 373 | |
Akito914 | 7:5cdd805ca8a3 | 374 | error = lift_stepTarg - lift_targ; |
Akito914 | 7:5cdd805ca8a3 | 375 | if(lift_stepMoving){ |
Akito914 | 7:5cdd805ca8a3 | 376 | if(error > lift_up_speed){ |
Akito914 | 7:5cdd805ca8a3 | 377 | lift_targ += lift_up_speed; |
Akito914 | 7:5cdd805ca8a3 | 378 | } |
Akito914 | 7:5cdd805ca8a3 | 379 | else if(error < lift_down_speed * -1){ |
Akito914 | 7:5cdd805ca8a3 | 380 | lift_targ -= lift_down_speed; |
Akito914 | 7:5cdd805ca8a3 | 381 | } |
Akito914 | 7:5cdd805ca8a3 | 382 | else{ |
Akito914 | 7:5cdd805ca8a3 | 383 | lift_targ = lift_stepTarg; |
Akito914 | 7:5cdd805ca8a3 | 384 | lift_stepMoving = false; |
Akito914 | 7:5cdd805ca8a3 | 385 | } |
Akito914 | 2:c9de02d6d154 | 386 | } |
Akito914 | 2:c9de02d6d154 | 387 | else{ |
Akito914 | 7:5cdd805ca8a3 | 388 | if(lift_inc < 0){ |
Akito914 | 7:5cdd805ca8a3 | 389 | if(lift_min < lift_targ + lift_inc)lift_targ += lift_inc; |
Akito914 | 7:5cdd805ca8a3 | 390 | } |
Akito914 | 7:5cdd805ca8a3 | 391 | else if(lift_inc > 0){ |
Akito914 | 7:5cdd805ca8a3 | 392 | if(lift_max > lift_targ + lift_inc)lift_targ += lift_inc; |
Akito914 | 7:5cdd805ca8a3 | 393 | } |
Akito914 | 2:c9de02d6d154 | 394 | } |
sawai | 0:6da7d0e457a2 | 395 | } |
Akito914 | 2:c9de02d6d154 | 396 | else{ |
Akito914 | 7:5cdd805ca8a3 | 397 | prescaler_count += 1; |
sawai | 0:6da7d0e457a2 | 398 | } |
Akito914 | 2:c9de02d6d154 | 399 | |
Akito914 | 4:5591d3c8a761 | 400 | meter = lift_meter.read_u16() >> 6; |
Akito914 | 6:22ad12682a5d | 401 | #if LIFT_METER_INVERSION |
Akito914 | 6:22ad12682a5d | 402 | meter = 1023 - meter; |
Akito914 | 6:22ad12682a5d | 403 | #endif |
Akito914 | 6:22ad12682a5d | 404 | duty = lift_pid.get_pid(meter, lift_targ, 1); |
Akito914 | 1:baab5b88a142 | 405 | drive_motor(liftAddr, duty); |
Akito914 | 1:baab5b88a142 | 406 | |
Akito914 | 1:baab5b88a142 | 407 | } |
Akito914 | 1:baab5b88a142 | 408 | |
Akito914 | 1:baab5b88a142 | 409 | void spear_control(){ |
Akito914 | 4:5591d3c8a761 | 410 | const int spear_timeout_short = 100; |
Akito914 | 4:5591d3c8a761 | 411 | const int spear_timeout_long = 500; |
Akito914 | 1:baab5b88a142 | 412 | static int spear_state = 0; |
Akito914 | 4:5591d3c8a761 | 413 | static int spear_timeout_count = 0; |
Akito914 | 1:baab5b88a142 | 414 | signed int duty = 0; |
Akito914 | 1:baab5b88a142 | 415 | int sensor = spear_sensor; // リミット時に1 |
Akito914 | 1:baab5b88a142 | 416 | |
Akito914 | 1:baab5b88a142 | 417 | switch(spear_state){ |
Akito914 | 1:baab5b88a142 | 418 | case 0 : // 待ち |
Akito914 | 1:baab5b88a142 | 419 | if(spear){ |
Akito914 | 1:baab5b88a142 | 420 | spear_state = 1; |
Akito914 | 1:baab5b88a142 | 421 | } |
Akito914 | 1:baab5b88a142 | 422 | else{ |
Akito914 | 1:baab5b88a142 | 423 | if(sensor == 0){ |
Akito914 | 1:baab5b88a142 | 424 | duty = spearDuty * -1; |
Akito914 | 1:baab5b88a142 | 425 | } |
Akito914 | 1:baab5b88a142 | 426 | } |
Akito914 | 1:baab5b88a142 | 427 | break; |
Akito914 | 1:baab5b88a142 | 428 | case 1 : // 初期位置から抜け出す |
Akito914 | 1:baab5b88a142 | 429 | duty = spearDuty; |
Akito914 | 4:5591d3c8a761 | 430 | if(sensor != 0){ |
Akito914 | 4:5591d3c8a761 | 431 | if(spear_timeout_count < spear_timeout_short){ |
Akito914 | 4:5591d3c8a761 | 432 | spear_timeout_count += 1; |
Akito914 | 4:5591d3c8a761 | 433 | } |
Akito914 | 4:5591d3c8a761 | 434 | else{ |
Akito914 | 4:5591d3c8a761 | 435 | spear_timeout_count = 0; |
Akito914 | 4:5591d3c8a761 | 436 | spear_state = 0; |
Akito914 | 4:5591d3c8a761 | 437 | } |
Akito914 | 4:5591d3c8a761 | 438 | } |
Akito914 | 4:5591d3c8a761 | 439 | else{ |
Akito914 | 4:5591d3c8a761 | 440 | spear_timeout_count = 0; |
Akito914 | 1:baab5b88a142 | 441 | spear_state = 2; |
Akito914 | 1:baab5b88a142 | 442 | } |
Akito914 | 1:baab5b88a142 | 443 | break; |
Akito914 | 1:baab5b88a142 | 444 | case 2 : // 到達位置まで動かす |
Akito914 | 1:baab5b88a142 | 445 | if(sensor == 0){ |
Akito914 | 1:baab5b88a142 | 446 | duty = spearDuty; |
Akito914 | 4:5591d3c8a761 | 447 | if(spear_timeout_count < spear_timeout_long){ |
Akito914 | 4:5591d3c8a761 | 448 | spear_timeout_count += 1; |
Akito914 | 4:5591d3c8a761 | 449 | } |
Akito914 | 4:5591d3c8a761 | 450 | else{ |
Akito914 | 4:5591d3c8a761 | 451 | spear_timeout_count = 0; |
Akito914 | 4:5591d3c8a761 | 452 | spear_state = 3; |
Akito914 | 4:5591d3c8a761 | 453 | } |
Akito914 | 1:baab5b88a142 | 454 | } |
Akito914 | 1:baab5b88a142 | 455 | else{ |
Akito914 | 4:5591d3c8a761 | 456 | spear_timeout_count = 0; |
Akito914 | 1:baab5b88a142 | 457 | spear_state = 3; |
Akito914 | 1:baab5b88a142 | 458 | } |
Akito914 | 1:baab5b88a142 | 459 | break; |
Akito914 | 1:baab5b88a142 | 460 | case 3 : // 到達位置から抜け出す |
Akito914 | 1:baab5b88a142 | 461 | duty = spearDuty * -1; |
Akito914 | 1:baab5b88a142 | 462 | if(sensor == 0){ |
Akito914 | 1:baab5b88a142 | 463 | spear_state = 4; |
Akito914 | 1:baab5b88a142 | 464 | } |
Akito914 | 1:baab5b88a142 | 465 | break; |
Akito914 | 1:baab5b88a142 | 466 | case 4 : // 初期位置まで戻る |
Akito914 | 1:baab5b88a142 | 467 | if(sensor == 0){ |
Akito914 | 1:baab5b88a142 | 468 | duty = spearDuty * -1; |
Akito914 | 1:baab5b88a142 | 469 | } |
Akito914 | 1:baab5b88a142 | 470 | else{ |
Akito914 | 1:baab5b88a142 | 471 | spear_state = 0; |
Akito914 | 1:baab5b88a142 | 472 | } |
Akito914 | 1:baab5b88a142 | 473 | break; |
Akito914 | 1:baab5b88a142 | 474 | default : |
Akito914 | 1:baab5b88a142 | 475 | break; |
Akito914 | 1:baab5b88a142 | 476 | } |
Akito914 | 1:baab5b88a142 | 477 | drive_motor(spearAddr, duty); |
Akito914 | 1:baab5b88a142 | 478 | } |
Akito914 | 1:baab5b88a142 | 479 | |
Akito914 | 1:baab5b88a142 | 480 | |
Akito914 | 1:baab5b88a142 | 481 | void control(){ |
Akito914 | 1:baab5b88a142 | 482 | |
Akito914 | 6:22ad12682a5d | 483 | drive_motor(EMO_Addr, 1); |
Akito914 | 6:22ad12682a5d | 484 | |
Akito914 | 6:22ad12682a5d | 485 | omuni.set_speed(speed_x, speed_y, omega, f); |
Akito914 | 6:22ad12682a5d | 486 | omuni.drive(); |
Akito914 | 6:22ad12682a5d | 487 | |
Akito914 | 1:baab5b88a142 | 488 | arm_control(); |
Akito914 | 1:baab5b88a142 | 489 | lift_control(); |
Akito914 | 1:baab5b88a142 | 490 | spear_control(); |
Akito914 | 1:baab5b88a142 | 491 | |
sawai | 0:6da7d0e457a2 | 492 | } |
sawai | 0:6da7d0e457a2 | 493 | |
sawai | 0:6da7d0e457a2 | 494 | int main() |
sawai | 0:6da7d0e457a2 | 495 | { |
Akito914 | 1:baab5b88a142 | 496 | int period_us = 0; |
Akito914 | 1:baab5b88a142 | 497 | int count = 0; |
Akito914 | 1:baab5b88a142 | 498 | |
Akito914 | 4:5591d3c8a761 | 499 | pc.baud(115200); |
Akito914 | 6:22ad12682a5d | 500 | com.baud(115200); |
Akito914 | 4:5591d3c8a761 | 501 | pc.printf("Hello!\n"); |
Akito914 | 4:5591d3c8a761 | 502 | com.attach(com_rx, Serial::RxIrq); |
Akito914 | 1:baab5b88a142 | 503 | |
Akito914 | 3:6223efea43fe | 504 | i2cMaster.frequency(400000); |
Akito914 | 1:baab5b88a142 | 505 | |
sawai | 0:6da7d0e457a2 | 506 | omuni.set_speed(0.0f, 0.0f); |
sawai | 0:6da7d0e457a2 | 507 | omuni.set_pid(0, 3.0f, 0.07f, 0.05f); |
Akito914 | 4:5591d3c8a761 | 508 | //omuni.set_pid(0, 6.0f, 0.14f, 0.10f); |
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 | 4:5591d3c8a761 | 520 | /* |
Akito914 | 4:5591d3c8a761 | 521 | soundOut.period_us(10); |
Akito914 | 4:5591d3c8a761 | 522 | for(count = 0; count < 500; count++){ |
Akito914 | 4:5591d3c8a761 | 523 | wait_ms(1); |
Akito914 | 4:5591d3c8a761 | 524 | soundOut.write(count / 100.0); |
Akito914 | 4:5591d3c8a761 | 525 | } |
Akito914 | 4:5591d3c8a761 | 526 | soundRenew.attach_us(soundRenew_intr, 63); |
Akito914 | 4:5591d3c8a761 | 527 | |
Akito914 | 4:5591d3c8a761 | 528 | while(soundEnd == false); |
Akito914 | 4:5591d3c8a761 | 529 | */ |
Akito914 | 1:baab5b88a142 | 530 | |
sawai | 0:6da7d0e457a2 | 531 | while(1) |
sawai | 0:6da7d0e457a2 | 532 | { |
Akito914 | 2:c9de02d6d154 | 533 | wait(0.001); |
sawai | 0:6da7d0e457a2 | 534 | |
Akito914 | 6:22ad12682a5d | 535 | if(comTimeout_state == false){ |
Akito914 | 6:22ad12682a5d | 536 | control(); |
Akito914 | 6:22ad12682a5d | 537 | } |
Akito914 | 6:22ad12682a5d | 538 | else{ |
Akito914 | 6:22ad12682a5d | 539 | drive_motor(EMO_Addr, 0); |
Akito914 | 6:22ad12682a5d | 540 | } |
Akito914 | 4:5591d3c8a761 | 541 | |
Akito914 | 1:baab5b88a142 | 542 | if(ems){ |
Akito914 | 1:baab5b88a142 | 543 | emergencyStop(); |
Akito914 | 1:baab5b88a142 | 544 | } |
sawai | 0:6da7d0e457a2 | 545 | } |
Akito914 | 1:baab5b88a142 | 546 | } |
Akito914 | 1:baab5b88a142 | 547 | |
Akito914 | 1:baab5b88a142 | 548 | int drive_motor(int address,signed int duty){ /* アドレスを指定してモータを駆動 */ |
Akito914 | 1:baab5b88a142 | 549 | char send_data; |
Akito914 | 1:baab5b88a142 | 550 | int ack; |
Akito914 | 1:baab5b88a142 | 551 | if((duty>127)||(duty<-128))return -1; /* 範囲外なら送信しない */ |
Akito914 | 1:baab5b88a142 | 552 | send_data=duty; |
Akito914 | 1:baab5b88a142 | 553 | ack=i2cMaster.write(address,&send_data,1); |
Akito914 | 1:baab5b88a142 | 554 | return ack; |
Akito914 | 1:baab5b88a142 | 555 | } |
Akito914 | 1:baab5b88a142 | 556 | |
Akito914 | 1:baab5b88a142 | 557 | void emergencyStop(){ |
Akito914 | 1:baab5b88a142 | 558 | drive_motor(omuniAddr[0], 0); |
Akito914 | 1:baab5b88a142 | 559 | drive_motor(omuniAddr[1], 0); |
Akito914 | 1:baab5b88a142 | 560 | drive_motor(omuniAddr[2], 0); |
Akito914 | 1:baab5b88a142 | 561 | drive_motor(armAddr[0], 0); |
Akito914 | 1:baab5b88a142 | 562 | drive_motor(armAddr[1], 0); |
Akito914 | 1:baab5b88a142 | 563 | drive_motor(liftAddr, 0); |
Akito914 | 1:baab5b88a142 | 564 | drive_motor(spearAddr, 0); |
Akito914 | 4:5591d3c8a761 | 565 | |
Akito914 | 4:5591d3c8a761 | 566 | drive_motor(EMO_Addr, 0); |
Akito914 | 4:5591d3c8a761 | 567 | |
Akito914 | 1:baab5b88a142 | 568 | while(1); |
Akito914 | 8:956a76b98da0 | 569 | |
Akito914 | 1:baab5b88a142 | 570 | } |
Akito914 | 1:baab5b88a142 | 571 | |
Akito914 | 1:baab5b88a142 | 572 |