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@5:12be2ac0f395, 2017-09-11 (annotated)

Committer:
Akito914
Date:
Mon Sep 11 05:37:59 2017 +0000
Revision:
5:12be2ac0f395
Parent:
4:5591d3c8a761
Child:
6:22ad12682a5d
??????????????;

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