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@3:6223efea43fe, 2017-08-28 (annotated)

Committer:
Akito914
Date:
Mon Aug 28 07:27:23 2017 +0000
Revision:
3:6223efea43fe
Parent:
2:c9de02d6d154
Child:
4:5591d3c8a761
???????????

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