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@8:956a76b98da0, 2017-09-19 (annotated)

Committer:
Akito914
Date:
Tue Sep 19 03:19:28 2017 +0000
Revision:
8:956a76b98da0
Parent:
7:5cdd805ca8a3
Child:
9:bf6d20813364
???????????????????????????????????

Who changed what in which revision?

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