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@6:22ad12682a5d, 2017-09-18 (annotated)

Committer:
Akito914
Date:
Mon Sep 18 11:51:23 2017 +0000
Revision:
6:22ad12682a5d
Parent:
5:12be2ac0f395
Child:
7:5cdd805ca8a3
???????????MD?????????????????????????; ???????????????????????????????

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 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 5:12be2ac0f395 12 const float omuni_speed_max = 2.0f;
Akito914 3:6223efea43fe 13 const float omega_max = 0.7 * 2 * 3.14159265f;
Akito914 5:12be2ac0f395 14 const float omega_f = 0.7 * 2 * 3.14159265f;
Akito914 3:6223efea43fe 15 const float wrap_radius = 1.0f;
Akito914 4:5591d3c8a761 16 const float wrap_speed = 1.8f;
Akito914 6:22ad12682a5d 17 const int8_t armDuty[] = {-100, -100};
Akito914 1:baab5b88a142 18 const signed int spearDuty = 127;
Akito914 6:22ad12682a5d 19 //const int32_t lift_preset_min = 200;
Akito914 6:22ad12682a5d 20 const int32_t lift_preset_min = 320; // Temporary use
Akito914 6:22ad12682a5d 21 const int32_t lift_preset_max = 480;
Akito914 3:6223efea43fe 22 const int32_t lift_preset[4] = {
Akito914 3:6223efea43fe 23 lift_preset_min,
Akito914 3:6223efea43fe 24 (lift_preset_max - lift_preset_min) / 3.0 + lift_preset_min,
Akito914 3:6223efea43fe 25 (lift_preset_max - lift_preset_min) * 2.0 / 3.0 + lift_preset_min,
Akito914 3:6223efea43fe 26 lift_preset_max,
Akito914 3:6223efea43fe 27 }; // 12bit 0 ~ 4095
Akito914 3:6223efea43fe 28 const int32_t lift_max = lift_preset_max;
Akito914 3:6223efea43fe 29 const int32_t lift_min = lift_preset_min;
Akito914 6:22ad12682a5d 30 const int32_t lift_up_speed = 2;
Akito914 6:22ad12682a5d 31 const int32_t lift_down_speed = 2;
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 6:22ad12682a5d 42 bool comTimeout_state = false;
Akito914 1:baab5b88a142 43
Akito914 1:baab5b88a142 44 bool ems = false;
Akito914 1:baab5b88a142 45
sawai 0:6da7d0e457a2 46 bool arm = false;
Akito914 1:baab5b88a142 47 bool spear = false;
Akito914 2:c9de02d6d154 48
Akito914 1:baab5b88a142 49 int lift_targ = lift_preset[0];
Akito914 2:c9de02d6d154 50 int lift_stepTarg;
Akito914 3:6223efea43fe 51 bool lift_stepMoving = false;
Akito914 1:baab5b88a142 52 bool first_receive = true;
Akito914 1:baab5b88a142 53 int past_lift_gray = 0b00;
Akito914 1:baab5b88a142 54 int lift_inc = 0;
Akito914 1:baab5b88a142 55
Akito914 4:5591d3c8a761 56 const float soundGain = 1.0;
Akito914 4:5591d3c8a761 57 int soundDataCount = 0;
Akito914 4:5591d3c8a761 58 bool soundEnd = false;
Akito914 4:5591d3c8a761 59
Akito914 6:22ad12682a5d 60 DigitalIn button(USER_BUTTON);
Akito914 1:baab5b88a142 61 DigitalOut led(LED1);
sawai 0:6da7d0e457a2 62
Akito914 4:5591d3c8a761 63 Serial pc(USBTX, USBRX);
Akito914 4:5591d3c8a761 64 Serial com(PA_11, PA_12);
sawai 0:6da7d0e457a2 65 I2C i2cMaster(D14, D15);
Akito914 1:baab5b88a142 66 omuni omuni(&i2cMaster, TIM1, TIM2, TIM3, 533, 2.0f, omuniAddr, 0.4704f, 0.1f);
Akito914 1:baab5b88a142 67 AnalogIn lift_meter(PC_0);
Akito914 1:baab5b88a142 68 DigitalIn spear_sensor(PC_3);
Akito914 1:baab5b88a142 69 Timer mesPeriod;
Akito914 1:baab5b88a142 70 cal_pid lift_pid;
Akito914 4:5591d3c8a761 71 Ticker comTimeout;
Akito914 3:6223efea43fe 72
Akito914 4:5591d3c8a761 73 FastPWM soundOut(PB_6);
Akito914 4:5591d3c8a761 74 Ticker soundRenew;
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 5:12be2ac0f395 113 comBuf_t comBuf;
Akito914 5:12be2ac0f395 114
Akito914 1:baab5b88a142 115
Akito914 1:baab5b88a142 116 int grayDiffer2bit(int gray, int origin){
Akito914 1:baab5b88a142 117
Akito914 1:baab5b88a142 118 switch(origin){
Akito914 1:baab5b88a142 119 case 0b00:
Akito914 1:baab5b88a142 120 switch(gray){
Akito914 1:baab5b88a142 121 case 0b01: return 1;
Akito914 1:baab5b88a142 122 case 0b10: return -1;
Akito914 1:baab5b88a142 123 default : return 0;
Akito914 1:baab5b88a142 124 }
Akito914 1:baab5b88a142 125 case 0b01:
Akito914 1:baab5b88a142 126 switch(gray){
Akito914 1:baab5b88a142 127 case 0b11: return 1;
Akito914 1:baab5b88a142 128 case 0b00: return -1;
Akito914 1:baab5b88a142 129 default : return 0;
Akito914 1:baab5b88a142 130 }
Akito914 1:baab5b88a142 131 case 0b11:
Akito914 1:baab5b88a142 132 switch(gray){
Akito914 1:baab5b88a142 133 case 0b10: return 1;
Akito914 1:baab5b88a142 134 case 0b01: return -1;
Akito914 1:baab5b88a142 135 default : return 0;
Akito914 1:baab5b88a142 136 }
Akito914 1:baab5b88a142 137 case 0b10:
Akito914 1:baab5b88a142 138 switch(gray){
Akito914 1:baab5b88a142 139 case 0b00: return 1;
Akito914 1:baab5b88a142 140 case 0b11: return -1;
Akito914 1:baab5b88a142 141 default : return 0;
Akito914 1:baab5b88a142 142 }
Akito914 1:baab5b88a142 143 }
Akito914 1:baab5b88a142 144 return 0;
Akito914 1:baab5b88a142 145 }
Akito914 1:baab5b88a142 146
Akito914 5:12be2ac0f395 147 bool checksum_check(comBuf_t *buf){
Akito914 5:12be2ac0f395 148 int sum = 0;
Akito914 5:12be2ac0f395 149
Akito914 5:12be2ac0f395 150 for(int count = 0; count < 9; count++){
Akito914 6:22ad12682a5d 151 sum += buf->list[count];
Akito914 5:12be2ac0f395 152 }
Akito914 6:22ad12682a5d 153 return (sum & 0b01111111) == buf->checksum;
Akito914 5:12be2ac0f395 154 }
Akito914 5:12be2ac0f395 155
sawai 0:6da7d0e457a2 156
Akito914 4:5591d3c8a761 157 void com_rx()
sawai 0:6da7d0e457a2 158 {
Akito914 5:12be2ac0f395 159 static int byteCount = 0;
Akito914 4:5591d3c8a761 160 char temp = com.getc();
Akito914 5:12be2ac0f395 161
Akito914 5:12be2ac0f395 162 if(temp == 0b10000000){
Akito914 5:12be2ac0f395 163 comBuf.list[0] = temp;
Akito914 5:12be2ac0f395 164 byteCount = 1;
Akito914 5:12be2ac0f395 165 }
Akito914 6:22ad12682a5d 166 else if((temp & 0b10000000) != 0){ // 想定外のデータ
Akito914 5:12be2ac0f395 167 byteCount = 0;
Akito914 5:12be2ac0f395 168 }
Akito914 5:12be2ac0f395 169 else if(byteCount > 0){
Akito914 5:12be2ac0f395 170
Akito914 5:12be2ac0f395 171 comBuf.list[byteCount] = temp;
Akito914 5:12be2ac0f395 172
Akito914 5:12be2ac0f395 173 if(byteCount < 9)byteCount += 1;
Akito914 5:12be2ac0f395 174 else{ // データは揃った
Akito914 6:22ad12682a5d 175
Akito914 5:12be2ac0f395 176 if(checksum_check(&comBuf)){
Akito914 5:12be2ac0f395 177 led = !led;
Akito914 5:12be2ac0f395 178 comTimeout_count = 0;
Akito914 5:12be2ac0f395 179 received_processing(&comBuf);
Akito914 5:12be2ac0f395 180 }
Akito914 5:12be2ac0f395 181 else{
Akito914 5:12be2ac0f395 182 byteCount = 0;
Akito914 5:12be2ac0f395 183 }
Akito914 5:12be2ac0f395 184 }
Akito914 5:12be2ac0f395 185
Akito914 5:12be2ac0f395 186 }
Akito914 5:12be2ac0f395 187
Akito914 5:12be2ac0f395 188 }
Akito914 5:12be2ac0f395 189
Akito914 5:12be2ac0f395 190 int getJoy7bit(int raw){
Akito914 5:12be2ac0f395 191 return raw - 64;
Akito914 5:12be2ac0f395 192 }
Akito914 5:12be2ac0f395 193
Akito914 5:12be2ac0f395 194 void received_processing(comBuf_t *buf){
Akito914 5:12be2ac0f395 195
Akito914 1:baab5b88a142 196 int lift_nearest_num = 0;
sawai 0:6da7d0e457a2 197
Akito914 5:12be2ac0f395 198 ems = buf->EMO != 0;
Akito914 1:baab5b88a142 199
Akito914 5:12be2ac0f395 200 speed_x = omuni_speed_max * getJoy7bit(buf->joyLX) / 64.0f;
Akito914 5:12be2ac0f395 201 speed_y = omuni_speed_max * getJoy7bit(buf->joyLY) / 64.0f;
sawai 0:6da7d0e457a2 202
Akito914 6:22ad12682a5d 203 if(abs(getJoy7bit(buf->joyRY)) < 20 && abs(getJoy7bit(buf->joyRX)) > 50){
Akito914 5:12be2ac0f395 204 f = 1;
Akito914 5:12be2ac0f395 205 omega = omega_f;
Akito914 6:22ad12682a5d 206 if(getJoy7bit(buf->joyRX) < 0)omega *= -1;
Akito914 5:12be2ac0f395 207 }
Akito914 6:22ad12682a5d 208 else if(buf->L2 >= 5 && buf->R2 < 5){
Akito914 6:22ad12682a5d 209 omega = omega_max * buf->L2 * -1.0f / 127.0f;
Akito914 5:12be2ac0f395 210 f = 0;
Akito914 5:12be2ac0f395 211 }
Akito914 5:12be2ac0f395 212 else if(buf->L2 < 5 && buf->R2 >= 5){
Akito914 6:22ad12682a5d 213 omega = omega_max * buf->R2 * 1.0f / 127.0f;
Akito914 6:22ad12682a5d 214 f = 0;
Akito914 6:22ad12682a5d 215 }
Akito914 6:22ad12682a5d 216 else{
Akito914 6:22ad12682a5d 217 omega = 0;
Akito914 5:12be2ac0f395 218 f = 0;
sawai 0:6da7d0e457a2 219 }
sawai 0:6da7d0e457a2 220
Akito914 5:12be2ac0f395 221 if(buf->R1 || buf->L1){
Akito914 5:12be2ac0f395 222 speed_x = wrap_speed;
Akito914 5:12be2ac0f395 223 speed_y = 0;
Akito914 5:12be2ac0f395 224 f = 0;
Akito914 5:12be2ac0f395 225 omega = wrap_speed / wrap_radius;
Akito914 5:12be2ac0f395 226 speed_x *= buf->L1 ? 1 : -1;
Akito914 5:12be2ac0f395 227 omega *= buf->L1 ? 1 : -1;
Akito914 5:12be2ac0f395 228 }
Akito914 5:12be2ac0f395 229
Akito914 5:12be2ac0f395 230 arm = buf->arm != 0;
Akito914 5:12be2ac0f395 231
Akito914 5:12be2ac0f395 232 spear = buf->spear != 0;
Akito914 5:12be2ac0f395 233
Akito914 5:12be2ac0f395 234 if(first_receive){
Akito914 5:12be2ac0f395 235 past_lift_gray = buf->lift_gray;
Akito914 5:12be2ac0f395 236 first_receive = false;
Akito914 5:12be2ac0f395 237 }
Akito914 5:12be2ac0f395 238 if(buf->lift_gray != past_lift_gray){ // 段階移動
Akito914 5:12be2ac0f395 239 int lift_currentTarg;
Akito914 5:12be2ac0f395 240 int lift_diff = 0;
Akito914 5:12be2ac0f395 241 int lift_nearest = 10000;
Akito914 5:12be2ac0f395 242 int lift_nextTarg;
Akito914 1:baab5b88a142 243
Akito914 5:12be2ac0f395 244 lift_currentTarg = lift_targ;
Akito914 5:12be2ac0f395 245 if(lift_stepMoving){
Akito914 5:12be2ac0f395 246 lift_nextTarg = lift_stepTarg;
Akito914 5:12be2ac0f395 247 }
Akito914 5:12be2ac0f395 248 else{
Akito914 5:12be2ac0f395 249 lift_nextTarg = lift_currentTarg;
Akito914 1:baab5b88a142 250 }
Akito914 1:baab5b88a142 251
Akito914 5:12be2ac0f395 252 lift_diff = grayDiffer2bit(buf->lift_gray, past_lift_gray);
Akito914 5:12be2ac0f395 253 for(int num = 0; num < 4; num++){ // 最寄りの段階
Akito914 5:12be2ac0f395 254 if(lift_nearest > abs(lift_preset[num] - lift_currentTarg)){
Akito914 5:12be2ac0f395 255 lift_nearest_num = num;
Akito914 5:12be2ac0f395 256 lift_nearest = abs(lift_preset[num] - lift_currentTarg);
Akito914 5:12be2ac0f395 257 }
Akito914 1:baab5b88a142 258 }
Akito914 5:12be2ac0f395 259 if(lift_diff != 0){
Akito914 5:12be2ac0f395 260 if(lift_diff == 1){
Akito914 5:12be2ac0f395 261 if(lift_preset[lift_nearest_num] >= lift_currentTarg){
Akito914 5:12be2ac0f395 262 if(lift_nearest_num < 3)lift_nextTarg = lift_preset[lift_nearest_num + 1];
Akito914 5:12be2ac0f395 263 }
Akito914 5:12be2ac0f395 264 else{
Akito914 5:12be2ac0f395 265 lift_nextTarg = lift_preset[lift_nearest_num];
Akito914 1:baab5b88a142 266 }
Akito914 1:baab5b88a142 267 }
Akito914 5:12be2ac0f395 268 else if(lift_diff == -1){
Akito914 5:12be2ac0f395 269 if(lift_preset[lift_nearest_num] <= lift_currentTarg){
Akito914 5:12be2ac0f395 270 if(lift_nearest_num > 0)lift_nextTarg = lift_preset[lift_nearest_num - 1];
Akito914 1:baab5b88a142 271 }
Akito914 5:12be2ac0f395 272 else{
Akito914 5:12be2ac0f395 273 lift_nextTarg = lift_preset[lift_nearest_num];
Akito914 1:baab5b88a142 274 }
Akito914 1:baab5b88a142 275 }
Akito914 3:6223efea43fe 276
Akito914 5:12be2ac0f395 277 lift_stepTarg = lift_nextTarg;
Akito914 5:12be2ac0f395 278 lift_stepMoving = true;
Akito914 5:12be2ac0f395 279
Akito914 1:baab5b88a142 280 }
Akito914 1:baab5b88a142 281
Akito914 5:12be2ac0f395 282 }
Akito914 5:12be2ac0f395 283 past_lift_gray = buf->lift_gray;
Akito914 5:12be2ac0f395 284
Akito914 6:22ad12682a5d 285 if(abs(getJoy7bit(buf->joyRY)) > 50 && abs(getJoy7bit(buf->joyRX)) < 20){
Akito914 5:12be2ac0f395 286 if(getJoy7bit(buf->joyRY) > 0)lift_inc = 1;
Akito914 5:12be2ac0f395 287 else lift_inc = -1;
Akito914 5:12be2ac0f395 288 }
Akito914 5:12be2ac0f395 289 else{
Akito914 5:12be2ac0f395 290 lift_inc = 0;
Akito914 5:12be2ac0f395 291 }
Akito914 1:baab5b88a142 292
Akito914 1:baab5b88a142 293 }
Akito914 1:baab5b88a142 294
Akito914 1:baab5b88a142 295
Akito914 4:5591d3c8a761 296 void soundRenew_intr(){
Akito914 4:5591d3c8a761 297
Akito914 4:5591d3c8a761 298 if(soundDataCount < soundData_size - 1000){
Akito914 4:5591d3c8a761 299
Akito914 4:5591d3c8a761 300 soundOut.write((soundData[soundDataCount] * soundGain / 256.0) + 0.5);
Akito914 4:5591d3c8a761 301
Akito914 4:5591d3c8a761 302 soundDataCount++;
Akito914 4:5591d3c8a761 303
Akito914 4:5591d3c8a761 304 }
Akito914 4:5591d3c8a761 305 else{
Akito914 4:5591d3c8a761 306 soundEnd = true;
Akito914 4:5591d3c8a761 307 }
Akito914 4:5591d3c8a761 308
Akito914 4:5591d3c8a761 309 }
Akito914 4:5591d3c8a761 310
Akito914 4:5591d3c8a761 311
Akito914 4:5591d3c8a761 312 void comTimeout_intr(){
Akito914 4:5591d3c8a761 313
Akito914 4:5591d3c8a761 314 if(comTimeout_count >= comTimeout_ms){
Akito914 4:5591d3c8a761 315 speed_x = 0;
Akito914 4:5591d3c8a761 316 speed_y = 0;
Akito914 4:5591d3c8a761 317 omega = 0;
Akito914 4:5591d3c8a761 318 arm = 0;
Akito914 4:5591d3c8a761 319 spear = 0;
Akito914 4:5591d3c8a761 320 lift_inc = 0;
Akito914 6:22ad12682a5d 321 comTimeout_state = true;
Akito914 4:5591d3c8a761 322 }
Akito914 4:5591d3c8a761 323 else{
Akito914 4:5591d3c8a761 324 comTimeout_count += 1;
Akito914 6:22ad12682a5d 325 comTimeout_state = false;
Akito914 4:5591d3c8a761 326 }
Akito914 4:5591d3c8a761 327
Akito914 4:5591d3c8a761 328 }
Akito914 4:5591d3c8a761 329
Akito914 4:5591d3c8a761 330
Akito914 1:baab5b88a142 331 void arm_control(){
Akito914 1:baab5b88a142 332 char armData[2] = {0};
Akito914 1:baab5b88a142 333
Akito914 1:baab5b88a142 334 armData[0] = arm? armDuty[0] : 0 ;
Akito914 1:baab5b88a142 335 armData[1] = arm? armDuty[1] : 0 ;
Akito914 1:baab5b88a142 336
Akito914 1:baab5b88a142 337 i2cMaster.write(armAddr[0], &armData[0], 1);
Akito914 1:baab5b88a142 338 i2cMaster.write(armAddr[1], &armData[1], 1);
Akito914 1:baab5b88a142 339
Akito914 1:baab5b88a142 340 }
Akito914 1:baab5b88a142 341
Akito914 1:baab5b88a142 342 void lift_control(){
Akito914 1:baab5b88a142 343 int32_t meter = 0;
Akito914 1:baab5b88a142 344 signed int duty = 0;
Akito914 3:6223efea43fe 345 int32_t error = 0;
Akito914 1:baab5b88a142 346
Akito914 3:6223efea43fe 347 error = lift_stepTarg - lift_targ;
Akito914 3:6223efea43fe 348 if(lift_stepMoving){
Akito914 3:6223efea43fe 349 if(error > lift_up_speed){
Akito914 3:6223efea43fe 350 lift_targ += lift_up_speed;
Akito914 3:6223efea43fe 351 }
Akito914 3:6223efea43fe 352 else if(error < lift_down_speed * -1){
Akito914 3:6223efea43fe 353 lift_targ -= lift_down_speed;
Akito914 2:c9de02d6d154 354 }
Akito914 2:c9de02d6d154 355 else{
Akito914 3:6223efea43fe 356 lift_targ = lift_stepTarg;
Akito914 3:6223efea43fe 357 lift_stepMoving = false;
Akito914 2:c9de02d6d154 358 }
sawai 0:6da7d0e457a2 359 }
Akito914 2:c9de02d6d154 360 else{
Akito914 2:c9de02d6d154 361 if(lift_inc < 0){
Akito914 2:c9de02d6d154 362 if(lift_min < lift_targ + lift_inc)lift_targ += lift_inc;
Akito914 2:c9de02d6d154 363 }
Akito914 2:c9de02d6d154 364 else if(lift_inc > 0){
Akito914 2:c9de02d6d154 365 if(lift_max > lift_targ + lift_inc)lift_targ += lift_inc;
Akito914 2:c9de02d6d154 366 }
sawai 0:6da7d0e457a2 367 }
Akito914 2:c9de02d6d154 368
Akito914 4:5591d3c8a761 369 meter = lift_meter.read_u16() >> 6;
Akito914 6:22ad12682a5d 370 #if LIFT_METER_INVERSION
Akito914 6:22ad12682a5d 371 meter = 1023 - meter;
Akito914 6:22ad12682a5d 372 #endif
Akito914 6:22ad12682a5d 373 duty = lift_pid.get_pid(meter, lift_targ, 1);
Akito914 1:baab5b88a142 374 drive_motor(liftAddr, duty);
Akito914 1:baab5b88a142 375
Akito914 1:baab5b88a142 376 }
Akito914 1:baab5b88a142 377
Akito914 1:baab5b88a142 378 void spear_control(){
Akito914 4:5591d3c8a761 379 const int spear_timeout_short = 100;
Akito914 4:5591d3c8a761 380 const int spear_timeout_long = 500;
Akito914 1:baab5b88a142 381 static int spear_state = 0;
Akito914 4:5591d3c8a761 382 static int spear_timeout_count = 0;
Akito914 1:baab5b88a142 383 signed int duty = 0;
Akito914 1:baab5b88a142 384 int sensor = spear_sensor; // リミット時に1
Akito914 1:baab5b88a142 385
Akito914 1:baab5b88a142 386 switch(spear_state){
Akito914 1:baab5b88a142 387 case 0 : // 待ち
Akito914 1:baab5b88a142 388 if(spear){
Akito914 1:baab5b88a142 389 spear_state = 1;
Akito914 1:baab5b88a142 390 }
Akito914 1:baab5b88a142 391 else{
Akito914 1:baab5b88a142 392 if(sensor == 0){
Akito914 1:baab5b88a142 393 duty = spearDuty * -1;
Akito914 1:baab5b88a142 394 }
Akito914 1:baab5b88a142 395 }
Akito914 1:baab5b88a142 396 break;
Akito914 1:baab5b88a142 397 case 1 : // 初期位置から抜け出す
Akito914 1:baab5b88a142 398 duty = spearDuty;
Akito914 4:5591d3c8a761 399 if(sensor != 0){
Akito914 4:5591d3c8a761 400 if(spear_timeout_count < spear_timeout_short){
Akito914 4:5591d3c8a761 401 spear_timeout_count += 1;
Akito914 4:5591d3c8a761 402 }
Akito914 4:5591d3c8a761 403 else{
Akito914 4:5591d3c8a761 404 spear_timeout_count = 0;
Akito914 4:5591d3c8a761 405 spear_state = 0;
Akito914 4:5591d3c8a761 406 }
Akito914 4:5591d3c8a761 407 }
Akito914 4:5591d3c8a761 408 else{
Akito914 4:5591d3c8a761 409 spear_timeout_count = 0;
Akito914 1:baab5b88a142 410 spear_state = 2;
Akito914 1:baab5b88a142 411 }
Akito914 1:baab5b88a142 412 break;
Akito914 1:baab5b88a142 413 case 2 : // 到達位置まで動かす
Akito914 1:baab5b88a142 414 if(sensor == 0){
Akito914 1:baab5b88a142 415 duty = spearDuty;
Akito914 4:5591d3c8a761 416 if(spear_timeout_count < spear_timeout_long){
Akito914 4:5591d3c8a761 417 spear_timeout_count += 1;
Akito914 4:5591d3c8a761 418 }
Akito914 4:5591d3c8a761 419 else{
Akito914 4:5591d3c8a761 420 spear_timeout_count = 0;
Akito914 4:5591d3c8a761 421 spear_state = 3;
Akito914 4:5591d3c8a761 422 }
Akito914 1:baab5b88a142 423 }
Akito914 1:baab5b88a142 424 else{
Akito914 4:5591d3c8a761 425 spear_timeout_count = 0;
Akito914 1:baab5b88a142 426 spear_state = 3;
Akito914 1:baab5b88a142 427 }
Akito914 1:baab5b88a142 428 break;
Akito914 1:baab5b88a142 429 case 3 : // 到達位置から抜け出す
Akito914 1:baab5b88a142 430 duty = spearDuty * -1;
Akito914 1:baab5b88a142 431 if(sensor == 0){
Akito914 1:baab5b88a142 432 spear_state = 4;
Akito914 1:baab5b88a142 433 }
Akito914 1:baab5b88a142 434 break;
Akito914 1:baab5b88a142 435 case 4 : // 初期位置まで戻る
Akito914 1:baab5b88a142 436 if(sensor == 0){
Akito914 1:baab5b88a142 437 duty = spearDuty * -1;
Akito914 1:baab5b88a142 438 }
Akito914 1:baab5b88a142 439 else{
Akito914 1:baab5b88a142 440 spear_state = 0;
Akito914 1:baab5b88a142 441 }
Akito914 1:baab5b88a142 442 break;
Akito914 1:baab5b88a142 443 default :
Akito914 1:baab5b88a142 444 break;
Akito914 1:baab5b88a142 445 }
Akito914 1:baab5b88a142 446 drive_motor(spearAddr, duty);
Akito914 1:baab5b88a142 447 }
Akito914 1:baab5b88a142 448
Akito914 1:baab5b88a142 449
Akito914 1:baab5b88a142 450 void control(){
Akito914 1:baab5b88a142 451
Akito914 6:22ad12682a5d 452 drive_motor(EMO_Addr, 1);
Akito914 6:22ad12682a5d 453
Akito914 6:22ad12682a5d 454 omuni.set_speed(speed_x, speed_y, omega, f);
Akito914 6:22ad12682a5d 455 omuni.drive();
Akito914 6:22ad12682a5d 456
Akito914 1:baab5b88a142 457 arm_control();
Akito914 1:baab5b88a142 458 lift_control();
Akito914 1:baab5b88a142 459 spear_control();
Akito914 1:baab5b88a142 460
sawai 0:6da7d0e457a2 461 }
sawai 0:6da7d0e457a2 462
sawai 0:6da7d0e457a2 463 int main()
sawai 0:6da7d0e457a2 464 {
Akito914 1:baab5b88a142 465 int period_us = 0;
Akito914 1:baab5b88a142 466 int count = 0;
Akito914 1:baab5b88a142 467
Akito914 4:5591d3c8a761 468 pc.baud(115200);
Akito914 6:22ad12682a5d 469 com.baud(115200);
Akito914 4:5591d3c8a761 470 pc.printf("Hello!\n");
Akito914 4:5591d3c8a761 471 com.attach(com_rx, Serial::RxIrq);
Akito914 1:baab5b88a142 472
Akito914 3:6223efea43fe 473 i2cMaster.frequency(400000);
Akito914 1:baab5b88a142 474
sawai 0:6da7d0e457a2 475 omuni.set_speed(0.0f, 0.0f);
sawai 0:6da7d0e457a2 476 omuni.set_pid(0, 3.0f, 0.07f, 0.05f);
Akito914 4:5591d3c8a761 477 //omuni.set_pid(0, 6.0f, 0.14f, 0.10f);
sawai 0:6da7d0e457a2 478 omuni.set_pid(1, 3.0f, 0.07f, 0.05f);
sawai 0:6da7d0e457a2 479 omuni.set_pid(2, 3.0f, 0.07f, 0.05f);
sawai 0:6da7d0e457a2 480
Akito914 4:5591d3c8a761 481 lift_pid.param(1.5, 0.0, 0.0);
Akito914 1:baab5b88a142 482 lift_pid.period(0.001);
Akito914 1:baab5b88a142 483 lift_pid.output(-127, 127);
Akito914 1:baab5b88a142 484
Akito914 4:5591d3c8a761 485 comTimeout.attach_us(comTimeout_intr, 1000);
Akito914 1:baab5b88a142 486
Akito914 1:baab5b88a142 487 led = 0;
Akito914 1:baab5b88a142 488
Akito914 4:5591d3c8a761 489 /*
Akito914 4:5591d3c8a761 490 soundOut.period_us(10);
Akito914 4:5591d3c8a761 491 for(count = 0; count < 500; count++){
Akito914 4:5591d3c8a761 492 wait_ms(1);
Akito914 4:5591d3c8a761 493 soundOut.write(count / 100.0);
Akito914 4:5591d3c8a761 494 }
Akito914 4:5591d3c8a761 495 soundRenew.attach_us(soundRenew_intr, 63);
Akito914 4:5591d3c8a761 496
Akito914 4:5591d3c8a761 497 while(soundEnd == false);
Akito914 4:5591d3c8a761 498 */
Akito914 1:baab5b88a142 499
sawai 0:6da7d0e457a2 500 while(1)
sawai 0:6da7d0e457a2 501 {
Akito914 2:c9de02d6d154 502 wait(0.001);
sawai 0:6da7d0e457a2 503
Akito914 6:22ad12682a5d 504 if(comTimeout_state == false){
Akito914 6:22ad12682a5d 505 control();
Akito914 6:22ad12682a5d 506 }
Akito914 6:22ad12682a5d 507 else{
Akito914 6:22ad12682a5d 508 drive_motor(EMO_Addr, 0);
Akito914 6:22ad12682a5d 509 }
Akito914 4:5591d3c8a761 510
Akito914 1:baab5b88a142 511 if(ems){
Akito914 1:baab5b88a142 512 emergencyStop();
Akito914 1:baab5b88a142 513 }
sawai 0:6da7d0e457a2 514 }
Akito914 1:baab5b88a142 515 }
Akito914 1:baab5b88a142 516
Akito914 1:baab5b88a142 517 int drive_motor(int address,signed int duty){ /* アドレスを指定してモータを駆動 */
Akito914 1:baab5b88a142 518 char send_data;
Akito914 1:baab5b88a142 519 int ack;
Akito914 1:baab5b88a142 520 if((duty>127)||(duty<-128))return -1; /* 範囲外なら送信しない */
Akito914 1:baab5b88a142 521 send_data=duty;
Akito914 1:baab5b88a142 522 ack=i2cMaster.write(address,&send_data,1);
Akito914 1:baab5b88a142 523 return ack;
Akito914 1:baab5b88a142 524 }
Akito914 1:baab5b88a142 525
Akito914 1:baab5b88a142 526 void emergencyStop(){
Akito914 1:baab5b88a142 527 drive_motor(omuniAddr[0], 0);
Akito914 1:baab5b88a142 528 drive_motor(omuniAddr[1], 0);
Akito914 1:baab5b88a142 529 drive_motor(omuniAddr[2], 0);
Akito914 1:baab5b88a142 530 drive_motor(armAddr[0], 0);
Akito914 1:baab5b88a142 531 drive_motor(armAddr[1], 0);
Akito914 1:baab5b88a142 532 drive_motor(liftAddr, 0);
Akito914 1:baab5b88a142 533 drive_motor(spearAddr, 0);
Akito914 4:5591d3c8a761 534
Akito914 4:5591d3c8a761 535 drive_motor(EMO_Addr, 0);
Akito914 4:5591d3c8a761 536
Akito914 1:baab5b88a142 537 while(1);
Akito914 1:baab5b88a142 538 }
Akito914 1:baab5b88a142 539
Akito914 1:baab5b88a142 540