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@2:c9de02d6d154, 2017-08-28 (annotated)

Committer:
Akito914
Date:
Mon Aug 28 01:39:14 2017 +0000
Revision:
2:c9de02d6d154
Parent:
1:baab5b88a142
Child:
3:6223efea43fe
????????????????????????

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