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@7:5cdd805ca8a3, 2017-09-19 (annotated)

Committer:
Akito914
Date:
Tue Sep 19 02:39:07 2017 +0000
Revision:
7:5cdd805ca8a3
Parent:
6:22ad12682a5d
Child:
8:956a76b98da0
?????????1/10???

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