![](/media/cache/profiles/1129cb8ba4665189d5a9bd861a2e28b9.jpg.50x50_q85.jpg)
GifuRobo B term
Dependencies: FastPWM cal_PID mbed omuni
Fork of omuni_speed_pid by
main.cpp@4:5591d3c8a761, 2017-09-11 (annotated)
- Committer:
- Akito914
- Date:
- Mon Sep 11 02:44:27 2017 +0000
- Revision:
- 4:5591d3c8a761
- Parent:
- 3:6223efea43fe
- Child:
- 5:12be2ac0f395
spear??????????
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 | 4:5591d3c8a761 | 8 | const int comTimeout_ms = 200; |
Akito914 | 4:5591d3c8a761 | 9 | const float omuni_speed[4] = {0, 1.0, 2.0, 3.0}; |
Akito914 | 3:6223efea43fe | 10 | const float omega_max = 0.7 * 2 * 3.14159265f; |
Akito914 | 3:6223efea43fe | 11 | const float wrap_radius = 1.0f; |
Akito914 | 4:5591d3c8a761 | 12 | const float wrap_speed = 1.8f; |
Akito914 | 1:baab5b88a142 | 13 | const int8_t armDuty[] = {-50, -50}; |
Akito914 | 1:baab5b88a142 | 14 | const signed int spearDuty = 127; |
Akito914 | 4:5591d3c8a761 | 15 | const int32_t lift_preset_min = 135; |
Akito914 | 4:5591d3c8a761 | 16 | const int32_t lift_preset_max = 450; |
Akito914 | 3:6223efea43fe | 17 | const int32_t lift_preset[4] = { |
Akito914 | 3:6223efea43fe | 18 | lift_preset_min, |
Akito914 | 3:6223efea43fe | 19 | (lift_preset_max - lift_preset_min) / 3.0 + lift_preset_min, |
Akito914 | 3:6223efea43fe | 20 | (lift_preset_max - lift_preset_min) * 2.0 / 3.0 + lift_preset_min, |
Akito914 | 3:6223efea43fe | 21 | lift_preset_max, |
Akito914 | 3:6223efea43fe | 22 | }; // 12bit 0 ~ 4095 |
Akito914 | 3:6223efea43fe | 23 | const int32_t lift_max = lift_preset_max; |
Akito914 | 3:6223efea43fe | 24 | const int32_t lift_min = lift_preset_min; |
Akito914 | 4:5591d3c8a761 | 25 | const int32_t lift_up_speed = 5; |
Akito914 | 4:5591d3c8a761 | 26 | const int32_t lift_down_speed = 5; |
Akito914 | 3:6223efea43fe | 27 | |
Akito914 | 1:baab5b88a142 | 28 | |
Akito914 | 1:baab5b88a142 | 29 | const int omuniAddr[] = {0x10, 0x12, 0x14}; // 0000 , 1000 , 0100 |
Akito914 | 1:baab5b88a142 | 30 | const int armAddr[] = {0x16, 0x18}; // 1100 , 0010 |
Akito914 | 1:baab5b88a142 | 31 | const int spearAddr = 0x1a; // 1010 |
Akito914 | 1:baab5b88a142 | 32 | const int liftAddr = 0x1c; // 0110 |
Akito914 | 4:5591d3c8a761 | 33 | const int EMO_Addr = 0x1e; // 1110 |
Akito914 | 4:5591d3c8a761 | 34 | |
Akito914 | 4:5591d3c8a761 | 35 | int comTimeout_count = 0; |
Akito914 | 1:baab5b88a142 | 36 | |
Akito914 | 1:baab5b88a142 | 37 | int drive_motor(int address,signed int duty); |
Akito914 | 1:baab5b88a142 | 38 | void emergencyStop(); |
Akito914 | 1:baab5b88a142 | 39 | |
Akito914 | 1:baab5b88a142 | 40 | bool ems = false; |
Akito914 | 1:baab5b88a142 | 41 | |
sawai | 0:6da7d0e457a2 | 42 | bool arm = false; |
Akito914 | 1:baab5b88a142 | 43 | bool spear = false; |
Akito914 | 2:c9de02d6d154 | 44 | |
Akito914 | 1:baab5b88a142 | 45 | int lift_targ = lift_preset[0]; |
Akito914 | 2:c9de02d6d154 | 46 | int lift_stepTarg; |
Akito914 | 3:6223efea43fe | 47 | bool lift_stepMoving = false; |
Akito914 | 1:baab5b88a142 | 48 | bool first_receive = true; |
Akito914 | 1:baab5b88a142 | 49 | int past_lift_gray = 0b00; |
Akito914 | 1:baab5b88a142 | 50 | int lift_inc = 0; |
Akito914 | 1:baab5b88a142 | 51 | |
Akito914 | 4:5591d3c8a761 | 52 | const float soundGain = 1.0; |
Akito914 | 4:5591d3c8a761 | 53 | int soundDataCount = 0; |
Akito914 | 4:5591d3c8a761 | 54 | bool soundEnd = false; |
Akito914 | 4:5591d3c8a761 | 55 | |
Akito914 | 1:baab5b88a142 | 56 | //DigitalIn button(USER_BUTTON); |
Akito914 | 1:baab5b88a142 | 57 | DigitalOut led(LED1); |
sawai | 0:6da7d0e457a2 | 58 | |
Akito914 | 4:5591d3c8a761 | 59 | Serial pc(USBTX, USBRX); |
Akito914 | 4:5591d3c8a761 | 60 | Serial com(PA_11, PA_12); |
sawai | 0:6da7d0e457a2 | 61 | I2C i2cMaster(D14, D15); |
Akito914 | 1:baab5b88a142 | 62 | omuni omuni(&i2cMaster, TIM1, TIM2, TIM3, 533, 2.0f, omuniAddr, 0.4704f, 0.1f); |
Akito914 | 1:baab5b88a142 | 63 | AnalogIn lift_meter(PC_0); |
Akito914 | 1:baab5b88a142 | 64 | DigitalIn spear_sensor(PC_3); |
Akito914 | 1:baab5b88a142 | 65 | Timer mesPeriod; |
Akito914 | 1:baab5b88a142 | 66 | cal_pid lift_pid; |
Akito914 | 4:5591d3c8a761 | 67 | Ticker comTimeout; |
Akito914 | 3:6223efea43fe | 68 | |
Akito914 | 4:5591d3c8a761 | 69 | FastPWM soundOut(PB_6); |
Akito914 | 4:5591d3c8a761 | 70 | Ticker soundRenew; |
Akito914 | 1:baab5b88a142 | 71 | |
Akito914 | 1:baab5b88a142 | 72 | float speed_x, speed_y, omega; |
Akito914 | 1:baab5b88a142 | 73 | bool f; |
sawai | 0:6da7d0e457a2 | 74 | |
Akito914 | 1:baab5b88a142 | 75 | typedef union{ |
Akito914 | 1:baab5b88a142 | 76 | struct{ |
Akito914 | 1:baab5b88a142 | 77 | unsigned angle :4; |
Akito914 | 1:baab5b88a142 | 78 | unsigned speed :2; |
Akito914 | 1:baab5b88a142 | 79 | unsigned byteNum0:2; |
Akito914 | 1:baab5b88a142 | 80 | }; |
Akito914 | 1:baab5b88a142 | 81 | struct{ |
Akito914 | 1:baab5b88a142 | 82 | unsigned wrap_R :1; |
Akito914 | 1:baab5b88a142 | 83 | unsigned wrap_L :1; |
Akito914 | 1:baab5b88a142 | 84 | unsigned coorSys :1; |
Akito914 | 1:baab5b88a142 | 85 | unsigned omega :2; |
Akito914 | 1:baab5b88a142 | 86 | unsigned dir :1; |
Akito914 | 1:baab5b88a142 | 87 | unsigned byteNum1 :2; |
Akito914 | 1:baab5b88a142 | 88 | }; |
Akito914 | 1:baab5b88a142 | 89 | struct{ |
Akito914 | 1:baab5b88a142 | 90 | unsigned lift_down :1; |
Akito914 | 1:baab5b88a142 | 91 | unsigned lift_up :1; |
Akito914 | 1:baab5b88a142 | 92 | unsigned lift_gray :2; |
Akito914 | 1:baab5b88a142 | 93 | unsigned spear :1; |
Akito914 | 1:baab5b88a142 | 94 | unsigned arm :1; |
Akito914 | 1:baab5b88a142 | 95 | unsigned byteNum2 :2; |
Akito914 | 1:baab5b88a142 | 96 | }; |
Akito914 | 1:baab5b88a142 | 97 | struct{ |
Akito914 | 1:baab5b88a142 | 98 | unsigned reserved2 :1; |
Akito914 | 1:baab5b88a142 | 99 | unsigned emo :1; |
Akito914 | 1:baab5b88a142 | 100 | unsigned reserved3 :4; |
Akito914 | 1:baab5b88a142 | 101 | unsigned byteNum3 :2; |
Akito914 | 1:baab5b88a142 | 102 | }; |
Akito914 | 1:baab5b88a142 | 103 | struct{ |
Akito914 | 1:baab5b88a142 | 104 | unsigned control :6; |
Akito914 | 1:baab5b88a142 | 105 | unsigned byteNum :2; |
Akito914 | 1:baab5b88a142 | 106 | }; |
Akito914 | 1:baab5b88a142 | 107 | struct{ |
Akito914 | 1:baab5b88a142 | 108 | unsigned bit0:1; |
Akito914 | 1:baab5b88a142 | 109 | unsigned bit1:1; |
Akito914 | 1:baab5b88a142 | 110 | unsigned bit2:1; |
Akito914 | 1:baab5b88a142 | 111 | unsigned bit3:1; |
Akito914 | 1:baab5b88a142 | 112 | unsigned bit4:1; |
Akito914 | 1:baab5b88a142 | 113 | unsigned bit5:1; |
Akito914 | 1:baab5b88a142 | 114 | unsigned bit6:1; |
Akito914 | 1:baab5b88a142 | 115 | unsigned bit7:1; |
Akito914 | 1:baab5b88a142 | 116 | }; |
Akito914 | 1:baab5b88a142 | 117 | int8_t data; |
Akito914 | 1:baab5b88a142 | 118 | }comData_t; |
Akito914 | 1:baab5b88a142 | 119 | |
Akito914 | 1:baab5b88a142 | 120 | |
Akito914 | 1:baab5b88a142 | 121 | comData_t RCData[4] = {0}; |
Akito914 | 1:baab5b88a142 | 122 | |
Akito914 | 1:baab5b88a142 | 123 | int grayDiffer2bit(int gray, int origin){ |
Akito914 | 1:baab5b88a142 | 124 | |
Akito914 | 1:baab5b88a142 | 125 | switch(origin){ |
Akito914 | 1:baab5b88a142 | 126 | case 0b00: |
Akito914 | 1:baab5b88a142 | 127 | switch(gray){ |
Akito914 | 1:baab5b88a142 | 128 | case 0b01: return 1; |
Akito914 | 1:baab5b88a142 | 129 | case 0b10: return -1; |
Akito914 | 1:baab5b88a142 | 130 | default : return 0; |
Akito914 | 1:baab5b88a142 | 131 | } |
Akito914 | 1:baab5b88a142 | 132 | case 0b01: |
Akito914 | 1:baab5b88a142 | 133 | switch(gray){ |
Akito914 | 1:baab5b88a142 | 134 | case 0b11: return 1; |
Akito914 | 1:baab5b88a142 | 135 | case 0b00: return -1; |
Akito914 | 1:baab5b88a142 | 136 | default : return 0; |
Akito914 | 1:baab5b88a142 | 137 | } |
Akito914 | 1:baab5b88a142 | 138 | case 0b11: |
Akito914 | 1:baab5b88a142 | 139 | switch(gray){ |
Akito914 | 1:baab5b88a142 | 140 | case 0b10: return 1; |
Akito914 | 1:baab5b88a142 | 141 | case 0b01: return -1; |
Akito914 | 1:baab5b88a142 | 142 | default : return 0; |
Akito914 | 1:baab5b88a142 | 143 | } |
Akito914 | 1:baab5b88a142 | 144 | case 0b10: |
Akito914 | 1:baab5b88a142 | 145 | switch(gray){ |
Akito914 | 1:baab5b88a142 | 146 | case 0b00: return 1; |
Akito914 | 1:baab5b88a142 | 147 | case 0b11: return -1; |
Akito914 | 1:baab5b88a142 | 148 | default : return 0; |
Akito914 | 1:baab5b88a142 | 149 | } |
Akito914 | 1:baab5b88a142 | 150 | } |
Akito914 | 1:baab5b88a142 | 151 | return 0; |
Akito914 | 1:baab5b88a142 | 152 | } |
Akito914 | 1:baab5b88a142 | 153 | |
sawai | 0:6da7d0e457a2 | 154 | |
Akito914 | 4:5591d3c8a761 | 155 | void com_rx() |
sawai | 0:6da7d0e457a2 | 156 | { |
Akito914 | 4:5591d3c8a761 | 157 | char temp = com.getc(); |
Akito914 | 1:baab5b88a142 | 158 | comData_t rcTemp; |
Akito914 | 1:baab5b88a142 | 159 | rcTemp.data = temp; |
Akito914 | 1:baab5b88a142 | 160 | float rad, speed; |
Akito914 | 1:baab5b88a142 | 161 | int lift_nearest_num = 0; |
sawai | 0:6da7d0e457a2 | 162 | |
Akito914 | 1:baab5b88a142 | 163 | |
Akito914 | 1:baab5b88a142 | 164 | RCData[rcTemp.byteNum] = rcTemp; |
sawai | 0:6da7d0e457a2 | 165 | |
Akito914 | 1:baab5b88a142 | 166 | /* |
Akito914 | 4:5591d3c8a761 | 167 | pc.printf("%d",rcTemp.bit7); |
Akito914 | 4:5591d3c8a761 | 168 | pc.printf("%d",rcTemp.bit6); |
Akito914 | 4:5591d3c8a761 | 169 | pc.printf("%d",rcTemp.bit5); |
Akito914 | 4:5591d3c8a761 | 170 | pc.printf("%d",rcTemp.bit4); |
Akito914 | 4:5591d3c8a761 | 171 | pc.printf("%d",rcTemp.bit3); |
Akito914 | 4:5591d3c8a761 | 172 | pc.printf("%d",rcTemp.bit2); |
Akito914 | 4:5591d3c8a761 | 173 | pc.printf("%d",rcTemp.bit1); |
Akito914 | 4:5591d3c8a761 | 174 | pc.printf("%d",rcTemp.bit0); |
Akito914 | 4:5591d3c8a761 | 175 | pc.printf("\n"); |
Akito914 | 1:baab5b88a142 | 176 | */ |
Akito914 | 4:5591d3c8a761 | 177 | //pc.printf("%d\n", rcTemp.byteNum); |
Akito914 | 4:5591d3c8a761 | 178 | //pc.printf("speed = %d, angle = %d\n",rcTemp.speed, rcTemp.angle); |
sawai | 0:6da7d0e457a2 | 179 | |
Akito914 | 1:baab5b88a142 | 180 | switch(temp >> 6){ |
Akito914 | 1:baab5b88a142 | 181 | case 0: |
Akito914 | 4:5591d3c8a761 | 182 | //pc.printf("speed = %d, angle = %d\n",rcTemp.speed, rcTemp.angle); |
Akito914 | 1:baab5b88a142 | 183 | break; |
Akito914 | 1:baab5b88a142 | 184 | case 1: |
Akito914 | 4:5591d3c8a761 | 185 | //pc.printf("dir = %d, omega = %d, coorSys = %d\n", rcTemp.dir, rcTemp.omega, rcTemp.coorSys); |
Akito914 | 1:baab5b88a142 | 186 | break; |
Akito914 | 1:baab5b88a142 | 187 | case 2: |
Akito914 | 4:5591d3c8a761 | 188 | //pc.printf("arm = %d, spear = %d, lift_gray = %d, lift_up = %d, lift_down = %d\n",rcTemp.arm, rcTemp.spear, rcTemp.lift_gray, rcTemp.lift_up, rcTemp.lift_down); |
Akito914 | 1:baab5b88a142 | 189 | break; |
Akito914 | 1:baab5b88a142 | 190 | case 3: |
Akito914 | 4:5591d3c8a761 | 191 | |
Akito914 | 1:baab5b88a142 | 192 | |
Akito914 | 1:baab5b88a142 | 193 | break; |
sawai | 0:6da7d0e457a2 | 194 | } |
sawai | 0:6da7d0e457a2 | 195 | |
Akito914 | 1:baab5b88a142 | 196 | if(rcTemp.byteNum == 3){ // データは揃った |
Akito914 | 1:baab5b88a142 | 197 | |
Akito914 | 3:6223efea43fe | 198 | led = !led; |
Akito914 | 4:5591d3c8a761 | 199 | comTimeout_count = 0; |
Akito914 | 1:baab5b88a142 | 200 | |
Akito914 | 1:baab5b88a142 | 201 | ems = RCData[3].emo != 0; |
Akito914 | 1:baab5b88a142 | 202 | |
Akito914 | 1:baab5b88a142 | 203 | rad = RCData[0].angle * 3.141592 / 8.0; |
Akito914 | 3:6223efea43fe | 204 | speed = omuni_speed[RCData[0].speed]; |
Akito914 | 3:6223efea43fe | 205 | if(RCData[0].speed == 0)speed = 0; |
Akito914 | 1:baab5b88a142 | 206 | speed_x = speed * cos(rad) * -1; |
Akito914 | 1:baab5b88a142 | 207 | speed_y = speed * sin(rad) * -1; |
Akito914 | 1:baab5b88a142 | 208 | |
Akito914 | 1:baab5b88a142 | 209 | f = RCData[1].coorSys; |
Akito914 | 1:baab5b88a142 | 210 | omega = RCData[1].omega * omega_max / 3.0f; |
Akito914 | 1:baab5b88a142 | 211 | omega *= RCData[1].dir ? 1 : -1; |
Akito914 | 1:baab5b88a142 | 212 | |
Akito914 | 1:baab5b88a142 | 213 | |
Akito914 | 1:baab5b88a142 | 214 | if(RCData[1].wrap_R || RCData[1].wrap_L){ |
Akito914 | 1:baab5b88a142 | 215 | speed_x = wrap_speed; |
Akito914 | 1:baab5b88a142 | 216 | speed_y = 0; |
Akito914 | 1:baab5b88a142 | 217 | f = 0; |
Akito914 | 1:baab5b88a142 | 218 | omega = wrap_speed / wrap_radius; |
Akito914 | 1:baab5b88a142 | 219 | speed_x *= RCData[1].wrap_L ? 1 : -1; |
Akito914 | 1:baab5b88a142 | 220 | omega *= RCData[1].wrap_L ? 1 : -1; |
Akito914 | 1:baab5b88a142 | 221 | } |
Akito914 | 1:baab5b88a142 | 222 | |
Akito914 | 1:baab5b88a142 | 223 | |
Akito914 | 1:baab5b88a142 | 224 | arm = RCData[2].arm != 0; |
Akito914 | 1:baab5b88a142 | 225 | |
Akito914 | 1:baab5b88a142 | 226 | spear = RCData[2].spear != 0; |
Akito914 | 1:baab5b88a142 | 227 | |
Akito914 | 1:baab5b88a142 | 228 | if(first_receive){ |
Akito914 | 1:baab5b88a142 | 229 | past_lift_gray = RCData[2].lift_gray; |
Akito914 | 1:baab5b88a142 | 230 | first_receive = false; |
Akito914 | 1:baab5b88a142 | 231 | } |
Akito914 | 1:baab5b88a142 | 232 | if(RCData[2].lift_gray != past_lift_gray){ // 段階移動 |
Akito914 | 2:c9de02d6d154 | 233 | int lift_currentTarg; |
Akito914 | 1:baab5b88a142 | 234 | int lift_diff = 0; |
Akito914 | 1:baab5b88a142 | 235 | int lift_nearest = 10000; |
Akito914 | 2:c9de02d6d154 | 236 | int lift_nextTarg; |
Akito914 | 2:c9de02d6d154 | 237 | |
Akito914 | 2:c9de02d6d154 | 238 | lift_currentTarg = lift_targ; |
Akito914 | 3:6223efea43fe | 239 | if(lift_stepMoving){ |
Akito914 | 3:6223efea43fe | 240 | lift_nextTarg = lift_stepTarg; |
Akito914 | 3:6223efea43fe | 241 | } |
Akito914 | 3:6223efea43fe | 242 | else{ |
Akito914 | 3:6223efea43fe | 243 | lift_nextTarg = lift_currentTarg; |
Akito914 | 3:6223efea43fe | 244 | } |
Akito914 | 2:c9de02d6d154 | 245 | |
Akito914 | 1:baab5b88a142 | 246 | lift_diff = grayDiffer2bit(RCData[2].lift_gray, past_lift_gray); |
Akito914 | 1:baab5b88a142 | 247 | for(int num = 0; num < 4; num++){ // 最寄りの段階 |
Akito914 | 2:c9de02d6d154 | 248 | if(lift_nearest > abs(lift_preset[num] - lift_currentTarg)){ |
Akito914 | 1:baab5b88a142 | 249 | lift_nearest_num = num; |
Akito914 | 2:c9de02d6d154 | 250 | lift_nearest = abs(lift_preset[num] - lift_currentTarg); |
Akito914 | 1:baab5b88a142 | 251 | } |
Akito914 | 1:baab5b88a142 | 252 | } |
Akito914 | 3:6223efea43fe | 253 | if(lift_diff != 0){ |
Akito914 | 3:6223efea43fe | 254 | if(lift_diff == 1){ |
Akito914 | 4:5591d3c8a761 | 255 | if(lift_preset[lift_nearest_num] >= lift_currentTarg){ |
Akito914 | 3:6223efea43fe | 256 | if(lift_nearest_num < 3)lift_nextTarg = lift_preset[lift_nearest_num + 1]; |
Akito914 | 3:6223efea43fe | 257 | } |
Akito914 | 3:6223efea43fe | 258 | else{ |
Akito914 | 3:6223efea43fe | 259 | lift_nextTarg = lift_preset[lift_nearest_num]; |
Akito914 | 3:6223efea43fe | 260 | } |
Akito914 | 1:baab5b88a142 | 261 | } |
Akito914 | 3:6223efea43fe | 262 | else if(lift_diff == -1){ |
Akito914 | 3:6223efea43fe | 263 | if(lift_preset[lift_nearest_num] <= lift_currentTarg){ |
Akito914 | 3:6223efea43fe | 264 | if(lift_nearest_num > 0)lift_nextTarg = lift_preset[lift_nearest_num - 1]; |
Akito914 | 3:6223efea43fe | 265 | } |
Akito914 | 3:6223efea43fe | 266 | else{ |
Akito914 | 3:6223efea43fe | 267 | lift_nextTarg = lift_preset[lift_nearest_num]; |
Akito914 | 3:6223efea43fe | 268 | } |
Akito914 | 1:baab5b88a142 | 269 | } |
Akito914 | 3:6223efea43fe | 270 | |
Akito914 | 3:6223efea43fe | 271 | lift_stepTarg = lift_nextTarg; |
Akito914 | 3:6223efea43fe | 272 | lift_stepMoving = true; |
Akito914 | 3:6223efea43fe | 273 | |
Akito914 | 1:baab5b88a142 | 274 | } |
Akito914 | 3:6223efea43fe | 275 | |
Akito914 | 1:baab5b88a142 | 276 | } |
Akito914 | 1:baab5b88a142 | 277 | past_lift_gray = RCData[2].lift_gray; |
Akito914 | 1:baab5b88a142 | 278 | |
Akito914 | 4:5591d3c8a761 | 279 | if(RCData[2].lift_down != 0)lift_inc = -1; |
Akito914 | 4:5591d3c8a761 | 280 | else if(RCData[2].lift_up != 0)lift_inc = 1; |
Akito914 | 1:baab5b88a142 | 281 | else lift_inc = 0; |
Akito914 | 1:baab5b88a142 | 282 | |
Akito914 | 1:baab5b88a142 | 283 | } // すっきりした // してない |
Akito914 | 1:baab5b88a142 | 284 | |
Akito914 | 1:baab5b88a142 | 285 | } |
Akito914 | 1:baab5b88a142 | 286 | |
Akito914 | 1:baab5b88a142 | 287 | |
Akito914 | 4:5591d3c8a761 | 288 | void soundRenew_intr(){ |
Akito914 | 4:5591d3c8a761 | 289 | |
Akito914 | 4:5591d3c8a761 | 290 | if(soundDataCount < soundData_size - 1000){ |
Akito914 | 4:5591d3c8a761 | 291 | |
Akito914 | 4:5591d3c8a761 | 292 | soundOut.write((soundData[soundDataCount] * soundGain / 256.0) + 0.5); |
Akito914 | 4:5591d3c8a761 | 293 | |
Akito914 | 4:5591d3c8a761 | 294 | soundDataCount++; |
Akito914 | 4:5591d3c8a761 | 295 | |
Akito914 | 4:5591d3c8a761 | 296 | } |
Akito914 | 4:5591d3c8a761 | 297 | else{ |
Akito914 | 4:5591d3c8a761 | 298 | soundEnd = true; |
Akito914 | 4:5591d3c8a761 | 299 | } |
Akito914 | 4:5591d3c8a761 | 300 | |
Akito914 | 4:5591d3c8a761 | 301 | } |
Akito914 | 4:5591d3c8a761 | 302 | |
Akito914 | 4:5591d3c8a761 | 303 | |
Akito914 | 4:5591d3c8a761 | 304 | void comTimeout_intr(){ |
Akito914 | 4:5591d3c8a761 | 305 | |
Akito914 | 4:5591d3c8a761 | 306 | if(comTimeout_count >= comTimeout_ms){ |
Akito914 | 4:5591d3c8a761 | 307 | speed_x = 0; |
Akito914 | 4:5591d3c8a761 | 308 | speed_y = 0; |
Akito914 | 4:5591d3c8a761 | 309 | omega = 0; |
Akito914 | 4:5591d3c8a761 | 310 | arm = 0; |
Akito914 | 4:5591d3c8a761 | 311 | spear = 0; |
Akito914 | 4:5591d3c8a761 | 312 | lift_inc = 0; |
Akito914 | 4:5591d3c8a761 | 313 | } |
Akito914 | 4:5591d3c8a761 | 314 | else{ |
Akito914 | 4:5591d3c8a761 | 315 | comTimeout_count += 1; |
Akito914 | 4:5591d3c8a761 | 316 | } |
Akito914 | 4:5591d3c8a761 | 317 | |
Akito914 | 4:5591d3c8a761 | 318 | } |
Akito914 | 4:5591d3c8a761 | 319 | |
Akito914 | 4:5591d3c8a761 | 320 | |
Akito914 | 1:baab5b88a142 | 321 | void arm_control(){ |
Akito914 | 1:baab5b88a142 | 322 | char armData[2] = {0}; |
Akito914 | 1:baab5b88a142 | 323 | |
Akito914 | 1:baab5b88a142 | 324 | armData[0] = arm? armDuty[0] : 0 ; |
Akito914 | 1:baab5b88a142 | 325 | armData[1] = arm? armDuty[1] : 0 ; |
Akito914 | 1:baab5b88a142 | 326 | |
Akito914 | 1:baab5b88a142 | 327 | i2cMaster.write(armAddr[0], &armData[0], 1); |
Akito914 | 1:baab5b88a142 | 328 | i2cMaster.write(armAddr[1], &armData[1], 1); |
Akito914 | 1:baab5b88a142 | 329 | |
Akito914 | 1:baab5b88a142 | 330 | } |
Akito914 | 1:baab5b88a142 | 331 | |
Akito914 | 1:baab5b88a142 | 332 | void lift_control(){ |
Akito914 | 1:baab5b88a142 | 333 | int32_t meter = 0; |
Akito914 | 1:baab5b88a142 | 334 | signed int duty = 0; |
Akito914 | 3:6223efea43fe | 335 | int32_t error = 0; |
Akito914 | 1:baab5b88a142 | 336 | |
Akito914 | 3:6223efea43fe | 337 | error = lift_stepTarg - lift_targ; |
Akito914 | 3:6223efea43fe | 338 | if(lift_stepMoving){ |
Akito914 | 3:6223efea43fe | 339 | if(error > lift_up_speed){ |
Akito914 | 3:6223efea43fe | 340 | lift_targ += lift_up_speed; |
Akito914 | 3:6223efea43fe | 341 | } |
Akito914 | 3:6223efea43fe | 342 | else if(error < lift_down_speed * -1){ |
Akito914 | 3:6223efea43fe | 343 | lift_targ -= lift_down_speed; |
Akito914 | 2:c9de02d6d154 | 344 | } |
Akito914 | 2:c9de02d6d154 | 345 | else{ |
Akito914 | 3:6223efea43fe | 346 | lift_targ = lift_stepTarg; |
Akito914 | 3:6223efea43fe | 347 | lift_stepMoving = false; |
Akito914 | 2:c9de02d6d154 | 348 | } |
sawai | 0:6da7d0e457a2 | 349 | } |
Akito914 | 2:c9de02d6d154 | 350 | else{ |
Akito914 | 2:c9de02d6d154 | 351 | if(lift_inc < 0){ |
Akito914 | 2:c9de02d6d154 | 352 | if(lift_min < lift_targ + lift_inc)lift_targ += lift_inc; |
Akito914 | 2:c9de02d6d154 | 353 | } |
Akito914 | 2:c9de02d6d154 | 354 | else if(lift_inc > 0){ |
Akito914 | 2:c9de02d6d154 | 355 | if(lift_max > lift_targ + lift_inc)lift_targ += lift_inc; |
Akito914 | 2:c9de02d6d154 | 356 | } |
sawai | 0:6da7d0e457a2 | 357 | } |
Akito914 | 2:c9de02d6d154 | 358 | |
Akito914 | 4:5591d3c8a761 | 359 | meter = lift_meter.read_u16() >> 6; |
Akito914 | 3:6223efea43fe | 360 | duty = lift_pid.get_pid(meter, lift_targ, -1); |
Akito914 | 1:baab5b88a142 | 361 | drive_motor(liftAddr, duty); |
Akito914 | 1:baab5b88a142 | 362 | |
Akito914 | 1:baab5b88a142 | 363 | } |
Akito914 | 1:baab5b88a142 | 364 | |
Akito914 | 1:baab5b88a142 | 365 | void spear_control(){ |
Akito914 | 4:5591d3c8a761 | 366 | const int spear_timeout_short = 100; |
Akito914 | 4:5591d3c8a761 | 367 | const int spear_timeout_long = 500; |
Akito914 | 1:baab5b88a142 | 368 | static int spear_state = 0; |
Akito914 | 4:5591d3c8a761 | 369 | static int spear_timeout_count = 0; |
Akito914 | 1:baab5b88a142 | 370 | signed int duty = 0; |
Akito914 | 1:baab5b88a142 | 371 | int sensor = spear_sensor; // リミット時に1 |
Akito914 | 1:baab5b88a142 | 372 | |
Akito914 | 1:baab5b88a142 | 373 | switch(spear_state){ |
Akito914 | 1:baab5b88a142 | 374 | case 0 : // 待ち |
Akito914 | 1:baab5b88a142 | 375 | if(spear){ |
Akito914 | 1:baab5b88a142 | 376 | spear_state = 1; |
Akito914 | 1:baab5b88a142 | 377 | } |
Akito914 | 1:baab5b88a142 | 378 | else{ |
Akito914 | 1:baab5b88a142 | 379 | if(sensor == 0){ |
Akito914 | 1:baab5b88a142 | 380 | duty = spearDuty * -1; |
Akito914 | 1:baab5b88a142 | 381 | } |
Akito914 | 1:baab5b88a142 | 382 | } |
Akito914 | 1:baab5b88a142 | 383 | break; |
Akito914 | 1:baab5b88a142 | 384 | case 1 : // 初期位置から抜け出す |
Akito914 | 1:baab5b88a142 | 385 | duty = spearDuty; |
Akito914 | 4:5591d3c8a761 | 386 | if(sensor != 0){ |
Akito914 | 4:5591d3c8a761 | 387 | if(spear_timeout_count < spear_timeout_short){ |
Akito914 | 4:5591d3c8a761 | 388 | spear_timeout_count += 1; |
Akito914 | 4:5591d3c8a761 | 389 | } |
Akito914 | 4:5591d3c8a761 | 390 | else{ |
Akito914 | 4:5591d3c8a761 | 391 | spear_timeout_count = 0; |
Akito914 | 4:5591d3c8a761 | 392 | spear_state = 0; |
Akito914 | 4:5591d3c8a761 | 393 | } |
Akito914 | 4:5591d3c8a761 | 394 | } |
Akito914 | 4:5591d3c8a761 | 395 | else{ |
Akito914 | 4:5591d3c8a761 | 396 | spear_timeout_count = 0; |
Akito914 | 1:baab5b88a142 | 397 | spear_state = 2; |
Akito914 | 1:baab5b88a142 | 398 | } |
Akito914 | 1:baab5b88a142 | 399 | break; |
Akito914 | 1:baab5b88a142 | 400 | case 2 : // 到達位置まで動かす |
Akito914 | 1:baab5b88a142 | 401 | if(sensor == 0){ |
Akito914 | 1:baab5b88a142 | 402 | duty = spearDuty; |
Akito914 | 4:5591d3c8a761 | 403 | if(spear_timeout_count < spear_timeout_long){ |
Akito914 | 4:5591d3c8a761 | 404 | spear_timeout_count += 1; |
Akito914 | 4:5591d3c8a761 | 405 | } |
Akito914 | 4:5591d3c8a761 | 406 | else{ |
Akito914 | 4:5591d3c8a761 | 407 | spear_timeout_count = 0; |
Akito914 | 4:5591d3c8a761 | 408 | spear_state = 3; |
Akito914 | 4:5591d3c8a761 | 409 | } |
Akito914 | 1:baab5b88a142 | 410 | } |
Akito914 | 1:baab5b88a142 | 411 | else{ |
Akito914 | 4:5591d3c8a761 | 412 | spear_timeout_count = 0; |
Akito914 | 1:baab5b88a142 | 413 | spear_state = 3; |
Akito914 | 1:baab5b88a142 | 414 | } |
Akito914 | 1:baab5b88a142 | 415 | break; |
Akito914 | 1:baab5b88a142 | 416 | case 3 : // 到達位置から抜け出す |
Akito914 | 1:baab5b88a142 | 417 | duty = spearDuty * -1; |
Akito914 | 1:baab5b88a142 | 418 | if(sensor == 0){ |
Akito914 | 1:baab5b88a142 | 419 | spear_state = 4; |
Akito914 | 1:baab5b88a142 | 420 | } |
Akito914 | 1:baab5b88a142 | 421 | break; |
Akito914 | 1:baab5b88a142 | 422 | case 4 : // 初期位置まで戻る |
Akito914 | 1:baab5b88a142 | 423 | if(sensor == 0){ |
Akito914 | 1:baab5b88a142 | 424 | duty = spearDuty * -1; |
Akito914 | 1:baab5b88a142 | 425 | } |
Akito914 | 1:baab5b88a142 | 426 | else{ |
Akito914 | 1:baab5b88a142 | 427 | spear_state = 0; |
Akito914 | 1:baab5b88a142 | 428 | } |
Akito914 | 1:baab5b88a142 | 429 | break; |
Akito914 | 1:baab5b88a142 | 430 | default : |
Akito914 | 1:baab5b88a142 | 431 | break; |
Akito914 | 1:baab5b88a142 | 432 | } |
Akito914 | 1:baab5b88a142 | 433 | drive_motor(spearAddr, duty); |
Akito914 | 1:baab5b88a142 | 434 | } |
Akito914 | 1:baab5b88a142 | 435 | |
Akito914 | 1:baab5b88a142 | 436 | |
Akito914 | 1:baab5b88a142 | 437 | void control(){ |
Akito914 | 1:baab5b88a142 | 438 | |
Akito914 | 1:baab5b88a142 | 439 | arm_control(); |
Akito914 | 1:baab5b88a142 | 440 | lift_control(); |
Akito914 | 1:baab5b88a142 | 441 | spear_control(); |
Akito914 | 1:baab5b88a142 | 442 | |
sawai | 0:6da7d0e457a2 | 443 | } |
sawai | 0:6da7d0e457a2 | 444 | |
sawai | 0:6da7d0e457a2 | 445 | int main() |
sawai | 0:6da7d0e457a2 | 446 | { |
Akito914 | 1:baab5b88a142 | 447 | int period_us = 0; |
Akito914 | 1:baab5b88a142 | 448 | int count = 0; |
Akito914 | 1:baab5b88a142 | 449 | |
Akito914 | 4:5591d3c8a761 | 450 | pc.baud(115200); |
Akito914 | 4:5591d3c8a761 | 451 | com.baud(9600); |
Akito914 | 4:5591d3c8a761 | 452 | pc.printf("Hello!\n"); |
Akito914 | 4:5591d3c8a761 | 453 | com.attach(com_rx, Serial::RxIrq); |
Akito914 | 1:baab5b88a142 | 454 | |
Akito914 | 3:6223efea43fe | 455 | i2cMaster.frequency(400000); |
Akito914 | 1:baab5b88a142 | 456 | |
sawai | 0:6da7d0e457a2 | 457 | omuni.set_speed(0.0f, 0.0f); |
sawai | 0:6da7d0e457a2 | 458 | omuni.set_pid(0, 3.0f, 0.07f, 0.05f); |
Akito914 | 4:5591d3c8a761 | 459 | //omuni.set_pid(0, 6.0f, 0.14f, 0.10f); |
sawai | 0:6da7d0e457a2 | 460 | omuni.set_pid(1, 3.0f, 0.07f, 0.05f); |
sawai | 0:6da7d0e457a2 | 461 | omuni.set_pid(2, 3.0f, 0.07f, 0.05f); |
sawai | 0:6da7d0e457a2 | 462 | |
Akito914 | 4:5591d3c8a761 | 463 | lift_pid.param(1.5, 0.0, 0.0); |
Akito914 | 1:baab5b88a142 | 464 | lift_pid.period(0.001); |
Akito914 | 1:baab5b88a142 | 465 | lift_pid.output(-127, 127); |
Akito914 | 1:baab5b88a142 | 466 | |
Akito914 | 4:5591d3c8a761 | 467 | comTimeout.attach_us(comTimeout_intr, 1000); |
Akito914 | 1:baab5b88a142 | 468 | |
Akito914 | 1:baab5b88a142 | 469 | led = 0; |
Akito914 | 1:baab5b88a142 | 470 | |
Akito914 | 4:5591d3c8a761 | 471 | /* |
Akito914 | 4:5591d3c8a761 | 472 | soundOut.period_us(10); |
Akito914 | 4:5591d3c8a761 | 473 | for(count = 0; count < 500; count++){ |
Akito914 | 4:5591d3c8a761 | 474 | wait_ms(1); |
Akito914 | 4:5591d3c8a761 | 475 | soundOut.write(count / 100.0); |
Akito914 | 4:5591d3c8a761 | 476 | } |
Akito914 | 4:5591d3c8a761 | 477 | soundRenew.attach_us(soundRenew_intr, 63); |
Akito914 | 4:5591d3c8a761 | 478 | |
Akito914 | 4:5591d3c8a761 | 479 | while(soundEnd == false); |
Akito914 | 4:5591d3c8a761 | 480 | */ |
Akito914 | 1:baab5b88a142 | 481 | |
sawai | 0:6da7d0e457a2 | 482 | while(1) |
sawai | 0:6da7d0e457a2 | 483 | { |
Akito914 | 2:c9de02d6d154 | 484 | wait(0.001); |
Akito914 | 1:baab5b88a142 | 485 | omuni.set_speed(speed_x, speed_y, omega, f); |
sawai | 0:6da7d0e457a2 | 486 | omuni.drive(); |
Akito914 | 1:baab5b88a142 | 487 | control(); |
sawai | 0:6da7d0e457a2 | 488 | |
Akito914 | 4:5591d3c8a761 | 489 | drive_motor(EMO_Addr, 1); |
Akito914 | 4:5591d3c8a761 | 490 | |
Akito914 | 1:baab5b88a142 | 491 | if(ems){ |
Akito914 | 1:baab5b88a142 | 492 | emergencyStop(); |
Akito914 | 1:baab5b88a142 | 493 | } |
sawai | 0:6da7d0e457a2 | 494 | } |
Akito914 | 1:baab5b88a142 | 495 | } |
Akito914 | 1:baab5b88a142 | 496 | |
Akito914 | 1:baab5b88a142 | 497 | int drive_motor(int address,signed int duty){ /* アドレスを指定してモータを駆動 */ |
Akito914 | 1:baab5b88a142 | 498 | char send_data; |
Akito914 | 1:baab5b88a142 | 499 | int ack; |
Akito914 | 1:baab5b88a142 | 500 | if((duty>127)||(duty<-128))return -1; /* 範囲外なら送信しない */ |
Akito914 | 1:baab5b88a142 | 501 | send_data=duty; |
Akito914 | 1:baab5b88a142 | 502 | ack=i2cMaster.write(address,&send_data,1); |
Akito914 | 1:baab5b88a142 | 503 | return ack; |
Akito914 | 1:baab5b88a142 | 504 | } |
Akito914 | 1:baab5b88a142 | 505 | |
Akito914 | 1:baab5b88a142 | 506 | void emergencyStop(){ |
Akito914 | 1:baab5b88a142 | 507 | drive_motor(omuniAddr[0], 0); |
Akito914 | 1:baab5b88a142 | 508 | drive_motor(omuniAddr[1], 0); |
Akito914 | 1:baab5b88a142 | 509 | drive_motor(omuniAddr[2], 0); |
Akito914 | 1:baab5b88a142 | 510 | drive_motor(armAddr[0], 0); |
Akito914 | 1:baab5b88a142 | 511 | drive_motor(armAddr[1], 0); |
Akito914 | 1:baab5b88a142 | 512 | drive_motor(liftAddr, 0); |
Akito914 | 1:baab5b88a142 | 513 | drive_motor(spearAddr, 0); |
Akito914 | 4:5591d3c8a761 | 514 | |
Akito914 | 4:5591d3c8a761 | 515 | drive_motor(EMO_Addr, 0); |
Akito914 | 4:5591d3c8a761 | 516 | |
Akito914 | 1:baab5b88a142 | 517 | while(1); |
Akito914 | 1:baab5b88a142 | 518 | } |
Akito914 | 1:baab5b88a142 | 519 | |
Akito914 | 1:baab5b88a142 | 520 |