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@4:5591d3c8a761, 2017-09-11 (annotated)

Committer:
Akito914
Date:
Mon Sep 11 02:44:27 2017 +0000
Revision:
4:5591d3c8a761
Parent:
3:6223efea43fe
Child:
5:12be2ac0f395
spear??????????

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