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@1:baab5b88a142, 2017-08-23 (annotated)

Committer:
Akito914
Date:
Wed Aug 23 07:50:14 2017 +0000
Revision:
1:baab5b88a142
Parent:
0:6da7d0e457a2
Child:
2:c9de02d6d154
?????????;

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