![](/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@2:c9de02d6d154, 2017-08-28 (annotated)
- Committer:
- Akito914
- Date:
- Mon Aug 28 01:39:14 2017 +0000
- Revision:
- 2:c9de02d6d154
- Parent:
- 1:baab5b88a142
- Child:
- 3:6223efea43fe
????????????????????????
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" |
sawai | 0:6da7d0e457a2 | 5 | |
Akito914 | 1:baab5b88a142 | 6 | const float omega_max = 1.0 * 2 * 3.14159265f; |
Akito914 | 1:baab5b88a142 | 7 | const float wrap_radius = 1.5f; |
Akito914 | 1:baab5b88a142 | 8 | const float wrap_speed = 2.0f; |
Akito914 | 1:baab5b88a142 | 9 | const int8_t armDuty[] = {-50, -50}; |
Akito914 | 1:baab5b88a142 | 10 | const signed int spearDuty = 127; |
Akito914 | 2:c9de02d6d154 | 11 | const int32_t lift_preset[4] = {550, 1033, 1516, 2000}; // 12bit 0 ~ 4095 |
Akito914 | 2:c9de02d6d154 | 12 | const int32_t lift_max = 1980; |
Akito914 | 2:c9de02d6d154 | 13 | const int32_t lift_min = 550; |
Akito914 | 1:baab5b88a142 | 14 | |
Akito914 | 1:baab5b88a142 | 15 | const int omuniAddr[] = {0x10, 0x12, 0x14}; // 0000 , 1000 , 0100 |
Akito914 | 1:baab5b88a142 | 16 | const int armAddr[] = {0x16, 0x18}; // 1100 , 0010 |
Akito914 | 1:baab5b88a142 | 17 | const int spearAddr = 0x1a; // 1010 |
Akito914 | 1:baab5b88a142 | 18 | const int liftAddr = 0x1c; // 0110 |
Akito914 | 1:baab5b88a142 | 19 | |
Akito914 | 1:baab5b88a142 | 20 | int drive_motor(int address,signed int duty); |
Akito914 | 1:baab5b88a142 | 21 | void emergencyStop(); |
Akito914 | 1:baab5b88a142 | 22 | |
Akito914 | 1:baab5b88a142 | 23 | bool ems = false; |
Akito914 | 1:baab5b88a142 | 24 | |
sawai | 0:6da7d0e457a2 | 25 | bool arm = false; |
Akito914 | 1:baab5b88a142 | 26 | bool spear = false; |
Akito914 | 2:c9de02d6d154 | 27 | |
Akito914 | 1:baab5b88a142 | 28 | int lift_targ = lift_preset[0]; |
Akito914 | 2:c9de02d6d154 | 29 | int lift_stepTarg; |
Akito914 | 2:c9de02d6d154 | 30 | int lift_stepOrigin; |
Akito914 | 1:baab5b88a142 | 31 | bool first_receive = true; |
Akito914 | 1:baab5b88a142 | 32 | int past_lift_gray = 0b00; |
Akito914 | 1:baab5b88a142 | 33 | int lift_inc = 0; |
Akito914 | 2:c9de02d6d154 | 34 | int lift_task = 0; |
Akito914 | 2:c9de02d6d154 | 35 | int lift_time = 0; |
Akito914 | 1:baab5b88a142 | 36 | |
Akito914 | 1:baab5b88a142 | 37 | //DigitalIn button(USER_BUTTON); |
Akito914 | 1:baab5b88a142 | 38 | DigitalOut led(LED1); |
sawai | 0:6da7d0e457a2 | 39 | |
sawai | 0:6da7d0e457a2 | 40 | Serial p(USBTX, USBRX); |
sawai | 0:6da7d0e457a2 | 41 | Serial pc(PA_11, PA_12); |
sawai | 0:6da7d0e457a2 | 42 | I2C i2cMaster(D14, D15); |
Akito914 | 1:baab5b88a142 | 43 | omuni omuni(&i2cMaster, TIM1, TIM2, TIM3, 533, 2.0f, omuniAddr, 0.4704f, 0.1f); |
Akito914 | 1:baab5b88a142 | 44 | AnalogIn lift_meter(PC_0); |
Akito914 | 1:baab5b88a142 | 45 | DigitalIn spear_sensor(PC_3); |
Akito914 | 1:baab5b88a142 | 46 | Timer mesPeriod; |
Akito914 | 1:baab5b88a142 | 47 | cal_pid lift_pid; |
Akito914 | 2:c9de02d6d154 | 48 | timeBaseTrapezoidalMotionCal lift_motionCal(500, 500, 5000, 70000, -70000); |
Akito914 | 1:baab5b88a142 | 49 | |
Akito914 | 1:baab5b88a142 | 50 | float speed_x, speed_y, omega; |
Akito914 | 1:baab5b88a142 | 51 | bool f; |
sawai | 0:6da7d0e457a2 | 52 | |
Akito914 | 1:baab5b88a142 | 53 | typedef union{ |
Akito914 | 1:baab5b88a142 | 54 | struct{ |
Akito914 | 1:baab5b88a142 | 55 | unsigned angle :4; |
Akito914 | 1:baab5b88a142 | 56 | unsigned speed :2; |
Akito914 | 1:baab5b88a142 | 57 | unsigned byteNum0:2; |
Akito914 | 1:baab5b88a142 | 58 | }; |
Akito914 | 1:baab5b88a142 | 59 | struct{ |
Akito914 | 1:baab5b88a142 | 60 | unsigned wrap_R :1; |
Akito914 | 1:baab5b88a142 | 61 | unsigned wrap_L :1; |
Akito914 | 1:baab5b88a142 | 62 | unsigned coorSys :1; |
Akito914 | 1:baab5b88a142 | 63 | unsigned omega :2; |
Akito914 | 1:baab5b88a142 | 64 | unsigned dir :1; |
Akito914 | 1:baab5b88a142 | 65 | unsigned byteNum1 :2; |
Akito914 | 1:baab5b88a142 | 66 | }; |
Akito914 | 1:baab5b88a142 | 67 | struct{ |
Akito914 | 1:baab5b88a142 | 68 | unsigned lift_down :1; |
Akito914 | 1:baab5b88a142 | 69 | unsigned lift_up :1; |
Akito914 | 1:baab5b88a142 | 70 | unsigned lift_gray :2; |
Akito914 | 1:baab5b88a142 | 71 | unsigned spear :1; |
Akito914 | 1:baab5b88a142 | 72 | unsigned arm :1; |
Akito914 | 1:baab5b88a142 | 73 | unsigned byteNum2 :2; |
Akito914 | 1:baab5b88a142 | 74 | }; |
Akito914 | 1:baab5b88a142 | 75 | struct{ |
Akito914 | 1:baab5b88a142 | 76 | unsigned reserved2 :1; |
Akito914 | 1:baab5b88a142 | 77 | unsigned emo :1; |
Akito914 | 1:baab5b88a142 | 78 | unsigned reserved3 :4; |
Akito914 | 1:baab5b88a142 | 79 | unsigned byteNum3 :2; |
Akito914 | 1:baab5b88a142 | 80 | }; |
Akito914 | 1:baab5b88a142 | 81 | struct{ |
Akito914 | 1:baab5b88a142 | 82 | unsigned control :6; |
Akito914 | 1:baab5b88a142 | 83 | unsigned byteNum :2; |
Akito914 | 1:baab5b88a142 | 84 | }; |
Akito914 | 1:baab5b88a142 | 85 | struct{ |
Akito914 | 1:baab5b88a142 | 86 | unsigned bit0:1; |
Akito914 | 1:baab5b88a142 | 87 | unsigned bit1:1; |
Akito914 | 1:baab5b88a142 | 88 | unsigned bit2:1; |
Akito914 | 1:baab5b88a142 | 89 | unsigned bit3:1; |
Akito914 | 1:baab5b88a142 | 90 | unsigned bit4:1; |
Akito914 | 1:baab5b88a142 | 91 | unsigned bit5:1; |
Akito914 | 1:baab5b88a142 | 92 | unsigned bit6:1; |
Akito914 | 1:baab5b88a142 | 93 | unsigned bit7:1; |
Akito914 | 1:baab5b88a142 | 94 | }; |
Akito914 | 1:baab5b88a142 | 95 | int8_t data; |
Akito914 | 1:baab5b88a142 | 96 | }comData_t; |
Akito914 | 1:baab5b88a142 | 97 | |
Akito914 | 1:baab5b88a142 | 98 | |
Akito914 | 1:baab5b88a142 | 99 | comData_t RCData[4] = {0}; |
Akito914 | 1:baab5b88a142 | 100 | |
Akito914 | 1:baab5b88a142 | 101 | int grayDiffer2bit(int gray, int origin){ |
Akito914 | 1:baab5b88a142 | 102 | |
Akito914 | 1:baab5b88a142 | 103 | switch(origin){ |
Akito914 | 1:baab5b88a142 | 104 | case 0b00: |
Akito914 | 1:baab5b88a142 | 105 | switch(gray){ |
Akito914 | 1:baab5b88a142 | 106 | case 0b01: return 1; |
Akito914 | 1:baab5b88a142 | 107 | case 0b10: return -1; |
Akito914 | 1:baab5b88a142 | 108 | default : return 0; |
Akito914 | 1:baab5b88a142 | 109 | } |
Akito914 | 1:baab5b88a142 | 110 | case 0b01: |
Akito914 | 1:baab5b88a142 | 111 | switch(gray){ |
Akito914 | 1:baab5b88a142 | 112 | case 0b11: return 1; |
Akito914 | 1:baab5b88a142 | 113 | case 0b00: return -1; |
Akito914 | 1:baab5b88a142 | 114 | default : return 0; |
Akito914 | 1:baab5b88a142 | 115 | } |
Akito914 | 1:baab5b88a142 | 116 | case 0b11: |
Akito914 | 1:baab5b88a142 | 117 | switch(gray){ |
Akito914 | 1:baab5b88a142 | 118 | case 0b10: return 1; |
Akito914 | 1:baab5b88a142 | 119 | case 0b01: return -1; |
Akito914 | 1:baab5b88a142 | 120 | default : return 0; |
Akito914 | 1:baab5b88a142 | 121 | } |
Akito914 | 1:baab5b88a142 | 122 | case 0b10: |
Akito914 | 1:baab5b88a142 | 123 | switch(gray){ |
Akito914 | 1:baab5b88a142 | 124 | case 0b00: return 1; |
Akito914 | 1:baab5b88a142 | 125 | case 0b11: return -1; |
Akito914 | 1:baab5b88a142 | 126 | default : return 0; |
Akito914 | 1:baab5b88a142 | 127 | } |
Akito914 | 1:baab5b88a142 | 128 | } |
Akito914 | 1:baab5b88a142 | 129 | return 0; |
Akito914 | 1:baab5b88a142 | 130 | } |
Akito914 | 1:baab5b88a142 | 131 | |
sawai | 0:6da7d0e457a2 | 132 | |
sawai | 0:6da7d0e457a2 | 133 | void pc_rx() |
sawai | 0:6da7d0e457a2 | 134 | { |
Akito914 | 1:baab5b88a142 | 135 | char temp = pc.getc(); |
Akito914 | 1:baab5b88a142 | 136 | comData_t rcTemp; |
Akito914 | 1:baab5b88a142 | 137 | rcTemp.data = temp; |
Akito914 | 1:baab5b88a142 | 138 | float rad, speed; |
Akito914 | 1:baab5b88a142 | 139 | int lift_nearest_num = 0; |
sawai | 0:6da7d0e457a2 | 140 | |
Akito914 | 1:baab5b88a142 | 141 | |
Akito914 | 1:baab5b88a142 | 142 | RCData[rcTemp.byteNum] = rcTemp; |
sawai | 0:6da7d0e457a2 | 143 | |
Akito914 | 1:baab5b88a142 | 144 | /* |
Akito914 | 1:baab5b88a142 | 145 | p.printf("%d",rcTemp.bit7); |
Akito914 | 1:baab5b88a142 | 146 | p.printf("%d",rcTemp.bit6); |
Akito914 | 1:baab5b88a142 | 147 | p.printf("%d",rcTemp.bit5); |
Akito914 | 1:baab5b88a142 | 148 | p.printf("%d",rcTemp.bit4); |
Akito914 | 1:baab5b88a142 | 149 | p.printf("%d",rcTemp.bit3); |
Akito914 | 1:baab5b88a142 | 150 | p.printf("%d",rcTemp.bit2); |
Akito914 | 1:baab5b88a142 | 151 | p.printf("%d",rcTemp.bit1); |
Akito914 | 1:baab5b88a142 | 152 | p.printf("%d",rcTemp.bit0); |
Akito914 | 1:baab5b88a142 | 153 | p.printf("\n"); |
Akito914 | 1:baab5b88a142 | 154 | */ |
Akito914 | 1:baab5b88a142 | 155 | //p.printf("%d\n", rcTemp.byteNum); |
Akito914 | 1:baab5b88a142 | 156 | //p.printf("speed = %d, angle = %d\n",rcTemp.speed, rcTemp.angle); |
sawai | 0:6da7d0e457a2 | 157 | |
Akito914 | 1:baab5b88a142 | 158 | switch(temp >> 6){ |
Akito914 | 1:baab5b88a142 | 159 | case 0: |
Akito914 | 1:baab5b88a142 | 160 | //p.printf("speed = %d, angle = %d\n",rcTemp.speed, rcTemp.angle); |
Akito914 | 1:baab5b88a142 | 161 | break; |
Akito914 | 1:baab5b88a142 | 162 | case 1: |
Akito914 | 1:baab5b88a142 | 163 | //p.printf("dir = %d, omega = %d, coorSys = %d\n", rcTemp.dir, rcTemp.omega, rcTemp.coorSys); |
Akito914 | 1:baab5b88a142 | 164 | break; |
Akito914 | 1:baab5b88a142 | 165 | case 2: |
Akito914 | 1:baab5b88a142 | 166 | //p.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 | 167 | break; |
Akito914 | 1:baab5b88a142 | 168 | case 3: |
Akito914 | 1:baab5b88a142 | 169 | |
Akito914 | 1:baab5b88a142 | 170 | break; |
sawai | 0:6da7d0e457a2 | 171 | } |
sawai | 0:6da7d0e457a2 | 172 | |
Akito914 | 1:baab5b88a142 | 173 | if(rcTemp.byteNum == 3){ // データは揃った |
Akito914 | 1:baab5b88a142 | 174 | |
Akito914 | 2:c9de02d6d154 | 175 | // led = !led; |
Akito914 | 1:baab5b88a142 | 176 | |
Akito914 | 1:baab5b88a142 | 177 | ems = RCData[3].emo != 0; |
Akito914 | 1:baab5b88a142 | 178 | |
Akito914 | 1:baab5b88a142 | 179 | rad = RCData[0].angle * 3.141592 / 8.0; |
Akito914 | 1:baab5b88a142 | 180 | speed = 0.8 * RCData[0].speed; |
Akito914 | 1:baab5b88a142 | 181 | speed_x = speed * cos(rad) * -1; |
Akito914 | 1:baab5b88a142 | 182 | speed_y = speed * sin(rad) * -1; |
Akito914 | 1:baab5b88a142 | 183 | |
Akito914 | 1:baab5b88a142 | 184 | f = RCData[1].coorSys; |
Akito914 | 1:baab5b88a142 | 185 | omega = RCData[1].omega * omega_max / 3.0f; |
Akito914 | 1:baab5b88a142 | 186 | omega *= RCData[1].dir ? 1 : -1; |
Akito914 | 1:baab5b88a142 | 187 | |
Akito914 | 1:baab5b88a142 | 188 | |
Akito914 | 1:baab5b88a142 | 189 | if(RCData[1].wrap_R || RCData[1].wrap_L){ |
Akito914 | 1:baab5b88a142 | 190 | speed_x = wrap_speed; |
Akito914 | 1:baab5b88a142 | 191 | speed_y = 0; |
Akito914 | 1:baab5b88a142 | 192 | f = 0; |
Akito914 | 1:baab5b88a142 | 193 | omega = wrap_speed / wrap_radius; |
Akito914 | 1:baab5b88a142 | 194 | speed_x *= RCData[1].wrap_L ? 1 : -1; |
Akito914 | 1:baab5b88a142 | 195 | omega *= RCData[1].wrap_L ? 1 : -1; |
Akito914 | 1:baab5b88a142 | 196 | } |
Akito914 | 1:baab5b88a142 | 197 | |
Akito914 | 1:baab5b88a142 | 198 | |
Akito914 | 1:baab5b88a142 | 199 | arm = RCData[2].arm != 0; |
Akito914 | 1:baab5b88a142 | 200 | |
Akito914 | 1:baab5b88a142 | 201 | spear = RCData[2].spear != 0; |
Akito914 | 1:baab5b88a142 | 202 | |
Akito914 | 1:baab5b88a142 | 203 | if(first_receive){ |
Akito914 | 1:baab5b88a142 | 204 | past_lift_gray = RCData[2].lift_gray; |
Akito914 | 1:baab5b88a142 | 205 | first_receive = false; |
Akito914 | 1:baab5b88a142 | 206 | } |
Akito914 | 1:baab5b88a142 | 207 | if(RCData[2].lift_gray != past_lift_gray){ // 段階移動 |
Akito914 | 2:c9de02d6d154 | 208 | int lift_currentTarg; |
Akito914 | 1:baab5b88a142 | 209 | int lift_diff = 0; |
Akito914 | 1:baab5b88a142 | 210 | int lift_nearest = 10000; |
Akito914 | 2:c9de02d6d154 | 211 | int lift_nextTarg; |
Akito914 | 2:c9de02d6d154 | 212 | |
Akito914 | 2:c9de02d6d154 | 213 | lift_currentTarg = lift_targ; |
Akito914 | 2:c9de02d6d154 | 214 | lift_nextTarg = lift_currentTarg; |
Akito914 | 2:c9de02d6d154 | 215 | |
Akito914 | 1:baab5b88a142 | 216 | lift_diff = grayDiffer2bit(RCData[2].lift_gray, past_lift_gray); |
Akito914 | 1:baab5b88a142 | 217 | for(int num = 0; num < 4; num++){ // 最寄りの段階 |
Akito914 | 2:c9de02d6d154 | 218 | if(lift_nearest > abs(lift_preset[num] - lift_currentTarg)){ |
Akito914 | 1:baab5b88a142 | 219 | lift_nearest_num = num; |
Akito914 | 2:c9de02d6d154 | 220 | lift_nearest = abs(lift_preset[num] - lift_currentTarg); |
Akito914 | 1:baab5b88a142 | 221 | } |
Akito914 | 1:baab5b88a142 | 222 | } |
Akito914 | 1:baab5b88a142 | 223 | if(lift_diff == 1){ |
Akito914 | 2:c9de02d6d154 | 224 | if(lift_preset[lift_nearest_num] >= lift_currentTarg){ |
Akito914 | 2:c9de02d6d154 | 225 | if(lift_nearest_num < 3)lift_nextTarg = lift_preset[lift_nearest_num + 1]; |
Akito914 | 1:baab5b88a142 | 226 | } |
Akito914 | 1:baab5b88a142 | 227 | else{ |
Akito914 | 2:c9de02d6d154 | 228 | lift_nextTarg = lift_preset[lift_nearest_num]; |
Akito914 | 1:baab5b88a142 | 229 | } |
Akito914 | 1:baab5b88a142 | 230 | } |
Akito914 | 1:baab5b88a142 | 231 | else if(lift_diff == -1){ |
Akito914 | 2:c9de02d6d154 | 232 | if(lift_preset[lift_nearest_num] <= lift_currentTarg){ |
Akito914 | 2:c9de02d6d154 | 233 | if(lift_nearest_num > 0)lift_nextTarg = lift_preset[lift_nearest_num - 1]; |
Akito914 | 1:baab5b88a142 | 234 | } |
Akito914 | 1:baab5b88a142 | 235 | else{ |
Akito914 | 2:c9de02d6d154 | 236 | lift_nextTarg = lift_preset[lift_nearest_num]; |
Akito914 | 1:baab5b88a142 | 237 | } |
Akito914 | 1:baab5b88a142 | 238 | } |
Akito914 | 2:c9de02d6d154 | 239 | if(lift_task == 0){ |
Akito914 | 2:c9de02d6d154 | 240 | lift_stepOrigin = lift_currentTarg; |
Akito914 | 2:c9de02d6d154 | 241 | lift_stepTarg = lift_nextTarg; |
Akito914 | 2:c9de02d6d154 | 242 | lift_motionCal.setTarg(lift_stepTarg - lift_stepOrigin); |
Akito914 | 2:c9de02d6d154 | 243 | lift_time = 0; |
Akito914 | 2:c9de02d6d154 | 244 | lift_task += 1; |
Akito914 | 2:c9de02d6d154 | 245 | } |
Akito914 | 1:baab5b88a142 | 246 | } |
Akito914 | 1:baab5b88a142 | 247 | past_lift_gray = RCData[2].lift_gray; |
Akito914 | 1:baab5b88a142 | 248 | |
Akito914 | 2:c9de02d6d154 | 249 | if(RCData[2].lift_down != 0)lift_inc = -2; |
Akito914 | 2:c9de02d6d154 | 250 | else if(RCData[2].lift_up != 0)lift_inc = 2; |
Akito914 | 1:baab5b88a142 | 251 | else lift_inc = 0; |
Akito914 | 1:baab5b88a142 | 252 | |
Akito914 | 1:baab5b88a142 | 253 | } // すっきりした // してない |
Akito914 | 1:baab5b88a142 | 254 | |
Akito914 | 1:baab5b88a142 | 255 | } |
Akito914 | 1:baab5b88a142 | 256 | |
Akito914 | 1:baab5b88a142 | 257 | |
Akito914 | 1:baab5b88a142 | 258 | void arm_control(){ |
Akito914 | 1:baab5b88a142 | 259 | char armData[2] = {0}; |
Akito914 | 1:baab5b88a142 | 260 | |
Akito914 | 1:baab5b88a142 | 261 | armData[0] = arm? armDuty[0] : 0 ; |
Akito914 | 1:baab5b88a142 | 262 | armData[1] = arm? armDuty[1] : 0 ; |
Akito914 | 1:baab5b88a142 | 263 | |
Akito914 | 1:baab5b88a142 | 264 | i2cMaster.write(armAddr[0], &armData[0], 1); |
Akito914 | 1:baab5b88a142 | 265 | i2cMaster.write(armAddr[1], &armData[1], 1); |
Akito914 | 1:baab5b88a142 | 266 | |
Akito914 | 1:baab5b88a142 | 267 | } |
Akito914 | 1:baab5b88a142 | 268 | |
Akito914 | 1:baab5b88a142 | 269 | void lift_control(){ |
Akito914 | 1:baab5b88a142 | 270 | int32_t meter = 0; |
Akito914 | 1:baab5b88a142 | 271 | signed int duty = 0; |
Akito914 | 1:baab5b88a142 | 272 | |
Akito914 | 2:c9de02d6d154 | 273 | if(lift_task != 0){ |
Akito914 | 2:c9de02d6d154 | 274 | if(lift_targ != lift_stepTarg){ |
Akito914 | 2:c9de02d6d154 | 275 | lift_targ = lift_motionCal.calSteps(lift_time) + lift_stepOrigin; |
Akito914 | 2:c9de02d6d154 | 276 | lift_time += 1; |
Akito914 | 2:c9de02d6d154 | 277 | } |
Akito914 | 2:c9de02d6d154 | 278 | else{ |
Akito914 | 2:c9de02d6d154 | 279 | lift_task = 0; |
Akito914 | 2:c9de02d6d154 | 280 | } |
sawai | 0:6da7d0e457a2 | 281 | } |
Akito914 | 2:c9de02d6d154 | 282 | else{ |
Akito914 | 2:c9de02d6d154 | 283 | if(lift_inc < 0){ |
Akito914 | 2:c9de02d6d154 | 284 | if(lift_min < lift_targ + lift_inc)lift_targ += lift_inc; |
Akito914 | 2:c9de02d6d154 | 285 | } |
Akito914 | 2:c9de02d6d154 | 286 | else if(lift_inc > 0){ |
Akito914 | 2:c9de02d6d154 | 287 | if(lift_max > lift_targ + lift_inc)lift_targ += lift_inc; |
Akito914 | 2:c9de02d6d154 | 288 | } |
sawai | 0:6da7d0e457a2 | 289 | } |
Akito914 | 2:c9de02d6d154 | 290 | |
Akito914 | 1:baab5b88a142 | 291 | meter = lift_meter.read_u16() >> 4; |
Akito914 | 1:baab5b88a142 | 292 | duty = lift_pid.get_pid(meter, lift_targ); |
Akito914 | 1:baab5b88a142 | 293 | drive_motor(liftAddr, duty); |
Akito914 | 1:baab5b88a142 | 294 | |
Akito914 | 1:baab5b88a142 | 295 | } |
Akito914 | 1:baab5b88a142 | 296 | |
Akito914 | 1:baab5b88a142 | 297 | void spear_control(){ |
Akito914 | 1:baab5b88a142 | 298 | static int spear_state = 0; |
Akito914 | 1:baab5b88a142 | 299 | signed int duty = 0; |
Akito914 | 1:baab5b88a142 | 300 | int sensor = spear_sensor; // リミット時に1 |
Akito914 | 1:baab5b88a142 | 301 | |
Akito914 | 1:baab5b88a142 | 302 | switch(spear_state){ |
Akito914 | 1:baab5b88a142 | 303 | case 0 : // 待ち |
Akito914 | 1:baab5b88a142 | 304 | if(spear){ |
Akito914 | 1:baab5b88a142 | 305 | spear_state = 1; |
Akito914 | 1:baab5b88a142 | 306 | } |
Akito914 | 1:baab5b88a142 | 307 | else{ |
Akito914 | 1:baab5b88a142 | 308 | if(sensor == 0){ |
Akito914 | 1:baab5b88a142 | 309 | duty = spearDuty * -1; |
Akito914 | 1:baab5b88a142 | 310 | } |
Akito914 | 1:baab5b88a142 | 311 | } |
Akito914 | 1:baab5b88a142 | 312 | break; |
Akito914 | 1:baab5b88a142 | 313 | case 1 : // 初期位置から抜け出す |
Akito914 | 1:baab5b88a142 | 314 | duty = spearDuty; |
Akito914 | 1:baab5b88a142 | 315 | if(sensor == 0){ |
Akito914 | 1:baab5b88a142 | 316 | spear_state = 2; |
Akito914 | 1:baab5b88a142 | 317 | } |
Akito914 | 1:baab5b88a142 | 318 | break; |
Akito914 | 1:baab5b88a142 | 319 | case 2 : // 到達位置まで動かす |
Akito914 | 1:baab5b88a142 | 320 | if(sensor == 0){ |
Akito914 | 1:baab5b88a142 | 321 | duty = spearDuty; |
Akito914 | 1:baab5b88a142 | 322 | } |
Akito914 | 1:baab5b88a142 | 323 | else{ |
Akito914 | 1:baab5b88a142 | 324 | spear_state = 3; |
Akito914 | 1:baab5b88a142 | 325 | } |
Akito914 | 1:baab5b88a142 | 326 | break; |
Akito914 | 1:baab5b88a142 | 327 | case 3 : // 到達位置から抜け出す |
Akito914 | 1:baab5b88a142 | 328 | duty = spearDuty * -1; |
Akito914 | 1:baab5b88a142 | 329 | if(sensor == 0){ |
Akito914 | 1:baab5b88a142 | 330 | spear_state = 4; |
Akito914 | 1:baab5b88a142 | 331 | } |
Akito914 | 1:baab5b88a142 | 332 | break; |
Akito914 | 1:baab5b88a142 | 333 | case 4 : // 初期位置まで戻る |
Akito914 | 1:baab5b88a142 | 334 | if(sensor == 0){ |
Akito914 | 1:baab5b88a142 | 335 | duty = spearDuty * -1; |
Akito914 | 1:baab5b88a142 | 336 | } |
Akito914 | 1:baab5b88a142 | 337 | else{ |
Akito914 | 1:baab5b88a142 | 338 | spear_state = 0; |
Akito914 | 1:baab5b88a142 | 339 | } |
Akito914 | 1:baab5b88a142 | 340 | break; |
Akito914 | 1:baab5b88a142 | 341 | default : |
Akito914 | 1:baab5b88a142 | 342 | break; |
Akito914 | 1:baab5b88a142 | 343 | } |
Akito914 | 1:baab5b88a142 | 344 | drive_motor(spearAddr, duty); |
Akito914 | 1:baab5b88a142 | 345 | } |
Akito914 | 1:baab5b88a142 | 346 | |
Akito914 | 1:baab5b88a142 | 347 | |
Akito914 | 1:baab5b88a142 | 348 | void control(){ |
Akito914 | 1:baab5b88a142 | 349 | |
Akito914 | 1:baab5b88a142 | 350 | arm_control(); |
Akito914 | 1:baab5b88a142 | 351 | lift_control(); |
Akito914 | 1:baab5b88a142 | 352 | spear_control(); |
Akito914 | 1:baab5b88a142 | 353 | |
sawai | 0:6da7d0e457a2 | 354 | } |
sawai | 0:6da7d0e457a2 | 355 | |
sawai | 0:6da7d0e457a2 | 356 | int main() |
sawai | 0:6da7d0e457a2 | 357 | { |
Akito914 | 1:baab5b88a142 | 358 | int period_us = 0; |
Akito914 | 1:baab5b88a142 | 359 | int count = 0; |
Akito914 | 1:baab5b88a142 | 360 | |
sawai | 0:6da7d0e457a2 | 361 | p.baud(115200); |
sawai | 0:6da7d0e457a2 | 362 | pc.baud(9600); |
Akito914 | 1:baab5b88a142 | 363 | p.printf("Hello!\n"); |
sawai | 0:6da7d0e457a2 | 364 | pc.attach(pc_rx, Serial::RxIrq); |
Akito914 | 1:baab5b88a142 | 365 | |
Akito914 | 1:baab5b88a142 | 366 | //i2cMaster.frequency(400000); |
Akito914 | 1:baab5b88a142 | 367 | |
sawai | 0:6da7d0e457a2 | 368 | omuni.set_speed(0.0f, 0.0f); |
sawai | 0:6da7d0e457a2 | 369 | omuni.set_pid(0, 3.0f, 0.07f, 0.05f); |
sawai | 0:6da7d0e457a2 | 370 | omuni.set_pid(1, 3.0f, 0.07f, 0.05f); |
sawai | 0:6da7d0e457a2 | 371 | omuni.set_pid(2, 3.0f, 0.07f, 0.05f); |
sawai | 0:6da7d0e457a2 | 372 | |
Akito914 | 2:c9de02d6d154 | 373 | lift_pid.param(0.2, 0.0, 0.0); |
Akito914 | 1:baab5b88a142 | 374 | lift_pid.period(0.001); |
Akito914 | 1:baab5b88a142 | 375 | lift_pid.output(-127, 127); |
Akito914 | 1:baab5b88a142 | 376 | |
Akito914 | 1:baab5b88a142 | 377 | //control_loop.attach_us(&control, 1000); |
Akito914 | 1:baab5b88a142 | 378 | |
Akito914 | 1:baab5b88a142 | 379 | led = 0; |
Akito914 | 1:baab5b88a142 | 380 | |
Akito914 | 1:baab5b88a142 | 381 | //mesPeriod.start(); |
Akito914 | 1:baab5b88a142 | 382 | |
sawai | 0:6da7d0e457a2 | 383 | while(1) |
sawai | 0:6da7d0e457a2 | 384 | { |
Akito914 | 2:c9de02d6d154 | 385 | wait(0.001); |
Akito914 | 1:baab5b88a142 | 386 | omuni.set_speed(speed_x, speed_y, omega, f); |
sawai | 0:6da7d0e457a2 | 387 | omuni.drive(); |
Akito914 | 1:baab5b88a142 | 388 | control(); |
sawai | 0:6da7d0e457a2 | 389 | |
Akito914 | 1:baab5b88a142 | 390 | if(ems){ |
Akito914 | 1:baab5b88a142 | 391 | emergencyStop(); |
Akito914 | 1:baab5b88a142 | 392 | } |
sawai | 0:6da7d0e457a2 | 393 | } |
Akito914 | 1:baab5b88a142 | 394 | } |
Akito914 | 1:baab5b88a142 | 395 | |
Akito914 | 1:baab5b88a142 | 396 | int drive_motor(int address,signed int duty){ /* アドレスを指定してモータを駆動 */ |
Akito914 | 1:baab5b88a142 | 397 | char send_data; |
Akito914 | 1:baab5b88a142 | 398 | int ack; |
Akito914 | 1:baab5b88a142 | 399 | if((duty>127)||(duty<-128))return -1; /* 範囲外なら送信しない */ |
Akito914 | 1:baab5b88a142 | 400 | send_data=duty; |
Akito914 | 1:baab5b88a142 | 401 | ack=i2cMaster.write(address,&send_data,1); |
Akito914 | 1:baab5b88a142 | 402 | return ack; |
Akito914 | 1:baab5b88a142 | 403 | } |
Akito914 | 1:baab5b88a142 | 404 | |
Akito914 | 1:baab5b88a142 | 405 | |
Akito914 | 1:baab5b88a142 | 406 | void emergencyStop(){ |
Akito914 | 1:baab5b88a142 | 407 | drive_motor(omuniAddr[0], 0); |
Akito914 | 1:baab5b88a142 | 408 | drive_motor(omuniAddr[1], 0); |
Akito914 | 1:baab5b88a142 | 409 | drive_motor(omuniAddr[2], 0); |
Akito914 | 1:baab5b88a142 | 410 | drive_motor(armAddr[0], 0); |
Akito914 | 1:baab5b88a142 | 411 | drive_motor(armAddr[1], 0); |
Akito914 | 1:baab5b88a142 | 412 | drive_motor(liftAddr, 0); |
Akito914 | 1:baab5b88a142 | 413 | drive_motor(spearAddr, 0); |
Akito914 | 1:baab5b88a142 | 414 | while(1); |
Akito914 | 1:baab5b88a142 | 415 | } |
Akito914 | 1:baab5b88a142 | 416 | |
Akito914 | 1:baab5b88a142 | 417 |