Ryu Anzawa / Mbed OS 2019NHK_A_manual_red

to takasou

Dependencies:   SerialMultiByte QEI chatteringremoval omni_wheel prototype01 towelest PID ikarashiMDC_2byte_ver air2 OmniPosition PS3

main.cpp@1:e7413a7b013b, 2019-09-23 (annotated)

Committer:
piroro4560
Date:
Mon Sep 23 23:30:38 2019 +0000
Revision:
1:e7413a7b013b
Parent:
0:d3db2b0afc76
Child:
2:d01bc3dcb247
to takasou

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ec30109b 0:d3db2b0afc76 1 #include "mbed.h" //Red court
ec30109b 0:d3db2b0afc76 2 #include "ikarashiMDC.h"
ec30109b 0:d3db2b0afc76 3 #include "omni_wheel.h"
ec30109b 0:d3db2b0afc76 4 #include "proto01.h"
ec30109b 0:d3db2b0afc76 5 #include "OmniPosition.h"
ec30109b 0:d3db2b0afc76 6 #include "SerialMultiByte.h"
ec30109b 0:d3db2b0afc76 7 #include "PID.h"
ec30109b 0:d3db2b0afc76 8 #include "PS3.h"
ec30109b 0:d3db2b0afc76 9 #include "air.h"
ec30109b 0:d3db2b0afc76 10 #include "towelest.h"
ec30109b 0:d3db2b0afc76 11 #include "chatteringremoval.h"
ec30109b 0:d3db2b0afc76 12 #include "pinconfig_main.h"
ec30109b 0:d3db2b0afc76 13
ec30109b 0:d3db2b0afc76 14 PID angle(10.0,0.2,0.000095,0.01);
ec30109b 0:d3db2b0afc76 15 OmniWheel omni(4);
ec30109b 0:d3db2b0afc76 16 OmniPosition posi(main_0TX,main_0RX);
ec30109b 0:d3db2b0afc76 17 Serial serial(mdTX, mdRX, 115200);
ec30109b 0:d3db2b0afc76 18 RawSerial pc(USBTX, USBRX, 115200);
ec30109b 0:d3db2b0afc76 19 SerialMultiByte sensor(main_1TX,main_1RX);
ec30109b 0:d3db2b0afc76 20 towelest towel(&serial,2,schmitt_trigger_0,schmitt_trigger_1,schmitt_trigger_2);
ec30109b 0:d3db2b0afc76 21 air air;
ec30109b 0:d3db2b0afc76 22 PS3 ps3(FEPTX,FEPRX);
ec30109b 0:d3db2b0afc76 23 chatteringremoval button1(1.0);
ec30109b 0:d3db2b0afc76 24 DigitalOut sw1(hijoteisi);
ec30109b 0:d3db2b0afc76 25 DigitalOut LED(LED1);
ec30109b 0:d3db2b0afc76 26 Timer timer;
ec30109b 0:d3db2b0afc76 27
piroro4560 1:e7413a7b013b 28 int num = 0, X, Y, b[12], stick[4], trigger[2], loli[3], b+[12];
ec30109b 0:d3db2b0afc76 29 int X0,Y0,X1,Y1;
ec30109b 0:d3db2b0afc76 30 double valueX, valueY, value[4],mecha_power[4],spin_power=0, IRdistance[2],TFdistance[2],x2, y2;
ec30109b 0:d3db2b0afc76 31
ec30109b 0:d3db2b0afc76 32 union UnionBytes{
ec30109b 0:d3db2b0afc76 33 uint8_t bytes[28];
ec30109b 0:d3db2b0afc76 34 float IRdist[7]; //0~1
ec30109b 0:d3db2b0afc76 35 int32_t TFdist[7]; //2~3
ec30109b 0:d3db2b0afc76 36 int32_t pulses[7]; //4~6
ec30109b 0:d3db2b0afc76 37 };
ec30109b 0:d3db2b0afc76 38
ec30109b 0:d3db2b0afc76 39 ikarashiMDC motor[]=
ec30109b 0:d3db2b0afc76 40 {
ec30109b 0:d3db2b0afc76 41 ikarashiMDC(0,0,SM,&serial),
ec30109b 0:d3db2b0afc76 42 ikarashiMDC(0,1,SM,&serial),
ec30109b 0:d3db2b0afc76 43 ikarashiMDC(0,2,SM,&serial),
ec30109b 0:d3db2b0afc76 44 ikarashiMDC(0,3,SM,&serial),
ec30109b 0:d3db2b0afc76 45 ikarashiMDC(1,0,SM,&serial),
ec30109b 0:d3db2b0afc76 46 ikarashiMDC(1,1,SM,&serial),
ec30109b 0:d3db2b0afc76 47 ikarashiMDC(1,2,SM,&serial),
ec30109b 0:d3db2b0afc76 48 ikarashiMDC(1,3,SM,&serial)
ec30109b 0:d3db2b0afc76 49 };
ec30109b 0:d3db2b0afc76 50
ec30109b 0:d3db2b0afc76 51 Proto1 proto(500, 700, 0.4, 0.14);
ec30109b 0:d3db2b0afc76 52
piroro4560 1:e7413a7b013b 53
ec30109b 0:d3db2b0afc76 54 void getSensorData() //Receive sensor value
ec30109b 0:d3db2b0afc76 55 {
ec30109b 0:d3db2b0afc76 56 UnionBytes bytedata;
ec30109b 0:d3db2b0afc76 57 sensor.getData(bytedata.bytes);
ec30109b 0:d3db2b0afc76 58 for(int i=0; i<2; i++){
ec30109b 0:d3db2b0afc76 59 IRdistance[i] = bytedata.IRdist[i];
ec30109b 0:d3db2b0afc76 60 }
ec30109b 0:d3db2b0afc76 61 for(int i=0; i<2; i++){
ec30109b 0:d3db2b0afc76 62 TFdistance[i] = bytedata.TFdist[i+2];
ec30109b 0:d3db2b0afc76 63 }
ec30109b 0:d3db2b0afc76 64 for(int i=0; i<3; i++){
ec30109b 0:d3db2b0afc76 65 loli[i] = bytedata.pulses[i+4];
ec30109b 0:d3db2b0afc76 66 }
ec30109b 0:d3db2b0afc76 67 }
ec30109b 0:d3db2b0afc76 68
ec30109b 0:d3db2b0afc76 69 void clossmove0() //Adjustment,Airfront
ec30109b 0:d3db2b0afc76 70 {
ec30109b 0:d3db2b0afc76 71 if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
ec30109b 0:d3db2b0afc76 72 x2=0;
ec30109b 0:d3db2b0afc76 73 y2=0.3;
ec30109b 0:d3db2b0afc76 74 }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
ec30109b 0:d3db2b0afc76 75 x2=0;
ec30109b 0:d3db2b0afc76 76 y2=-0.3;
ec30109b 0:d3db2b0afc76 77 }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
ec30109b 0:d3db2b0afc76 78 x2=-0.3;
ec30109b 0:d3db2b0afc76 79 y2=0;
ec30109b 0:d3db2b0afc76 80 }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
ec30109b 0:d3db2b0afc76 81 x2=0.3;
ec30109b 0:d3db2b0afc76 82 y2=0;
ec30109b 0:d3db2b0afc76 83 }else{
ec30109b 0:d3db2b0afc76 84 x2=0;
ec30109b 0:d3db2b0afc76 85 y2=0;
ec30109b 0:d3db2b0afc76 86 }
ec30109b 0:d3db2b0afc76 87 }
ec30109b 0:d3db2b0afc76 88
ec30109b 0:d3db2b0afc76 89 void clossmove1() //Adjustment,Recoveryfront
ec30109b 0:d3db2b0afc76 90 {
ec30109b 0:d3db2b0afc76 91 if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
ec30109b 0:d3db2b0afc76 92 x2=0.3;
ec30109b 0:d3db2b0afc76 93 y2=0;
ec30109b 0:d3db2b0afc76 94 }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
ec30109b 0:d3db2b0afc76 95 x2=-0.3;
ec30109b 0:d3db2b0afc76 96 y2=0;
ec30109b 0:d3db2b0afc76 97 }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
ec30109b 0:d3db2b0afc76 98 x2=0;
ec30109b 0:d3db2b0afc76 99 y2=0.3;
ec30109b 0:d3db2b0afc76 100 }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
ec30109b 0:d3db2b0afc76 101 x2=0;
ec30109b 0:d3db2b0afc76 102 y2=-0.3;
ec30109b 0:d3db2b0afc76 103 }else{
ec30109b 0:d3db2b0afc76 104 x2=0;
ec30109b 0:d3db2b0afc76 105 y2=0;
ec30109b 0:d3db2b0afc76 106 }
ec30109b 0:d3db2b0afc76 107 }
ec30109b 0:d3db2b0afc76 108
ec30109b 0:d3db2b0afc76 109 void clossmove2() //Adjustment,Dropfront
ec30109b 0:d3db2b0afc76 110 {
ec30109b 0:d3db2b0afc76 111 if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
ec30109b 0:d3db2b0afc76 112 x2=0;
ec30109b 0:d3db2b0afc76 113 y2=-0.3;
ec30109b 0:d3db2b0afc76 114 }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
ec30109b 0:d3db2b0afc76 115 x2=0;
ec30109b 0:d3db2b0afc76 116 y2=0.3;
ec30109b 0:d3db2b0afc76 117 }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
ec30109b 0:d3db2b0afc76 118 x2=0.3;
ec30109b 0:d3db2b0afc76 119 y2=0;
ec30109b 0:d3db2b0afc76 120 }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
ec30109b 0:d3db2b0afc76 121 x2=-0.3;
ec30109b 0:d3db2b0afc76 122 y2=0;
ec30109b 0:d3db2b0afc76 123 }else{
ec30109b 0:d3db2b0afc76 124 x2=0;
ec30109b 0:d3db2b0afc76 125 y2=0;
ec30109b 0:d3db2b0afc76 126 }
ec30109b 0:d3db2b0afc76 127 }
ec30109b 0:d3db2b0afc76 128
ec30109b 0:d3db2b0afc76 129 void clossmove3() //Adjustment,Towelfront
ec30109b 0:d3db2b0afc76 130 {
ec30109b 0:d3db2b0afc76 131 if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
ec30109b 0:d3db2b0afc76 132 x2=-0.3;
ec30109b 0:d3db2b0afc76 133 y2=0;
ec30109b 0:d3db2b0afc76 134 }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
ec30109b 0:d3db2b0afc76 135 x2=0.3;
ec30109b 0:d3db2b0afc76 136 y2=0;
ec30109b 0:d3db2b0afc76 137 }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
ec30109b 0:d3db2b0afc76 138 x2=0;
ec30109b 0:d3db2b0afc76 139 y2=-0.3;
ec30109b 0:d3db2b0afc76 140 }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
ec30109b 0:d3db2b0afc76 141 x2=0;
ec30109b 0:d3db2b0afc76 142 y2=0.3;
ec30109b 0:d3db2b0afc76 143 }else{
ec30109b 0:d3db2b0afc76 144 x2=0;
ec30109b 0:d3db2b0afc76 145 y2=0;
ec30109b 0:d3db2b0afc76 146 }
ec30109b 0:d3db2b0afc76 147 }
ec30109b 0:d3db2b0afc76 148
ec30109b 0:d3db2b0afc76 149 void Recovery() //Recovery mechanism
ec30109b 0:d3db2b0afc76 150 {
ec30109b 0:d3db2b0afc76 151 /* 洗濯物を挟む機構 */
ec30109b 0:d3db2b0afc76 152 if((X0 > 170)&&(Y0 > 50 && Y0 < 200)) {
ec30109b 0:d3db2b0afc76 153 mecha_power[1] = 0.6;
ec30109b 0:d3db2b0afc76 154 }else if((X0 < 100)&&(Y0 > 50 && Y0 < 200)) {
ec30109b 0:d3db2b0afc76 155 mecha_power[1] = -0.6;
ec30109b 0:d3db2b0afc76 156 }else{
ec30109b 0:d3db2b0afc76 157 mecha_power[1] = 0;
ec30109b 0:d3db2b0afc76 158 }
ec30109b 0:d3db2b0afc76 159 if((X1 > 170)&&(Y1 > 50 && Y1 < 200)) {
ec30109b 0:d3db2b0afc76 160 mecha_power[2] = 0.6;
ec30109b 0:d3db2b0afc76 161 }else if((X1 < 100)&&(Y1 > 50 && Y1 < 200)) {
ec30109b 0:d3db2b0afc76 162 mecha_power[2] = -0.6;
ec30109b 0:d3db2b0afc76 163 }else{
ec30109b 0:d3db2b0afc76 164 mecha_power[2] = 0;
ec30109b 0:d3db2b0afc76 165 }
ec30109b 0:d3db2b0afc76 166 /* 洗濯物を挟む機構の首 */
ec30109b 0:d3db2b0afc76 167 if (b[10] == 1 && b[11] == 0) {
ec30109b 0:d3db2b0afc76 168 mecha_power[0] = 0.3;
ec30109b 0:d3db2b0afc76 169 } else if (b[10] == 0 && b[11] == 1) {
ec30109b 0:d3db2b0afc76 170 mecha_power[0] = -0.3;
ec30109b 0:d3db2b0afc76 171 } else {
ec30109b 0:d3db2b0afc76 172 mecha_power[0] =0.0;
ec30109b 0:d3db2b0afc76 173 }
ec30109b 0:d3db2b0afc76 174 mecha_power[3] = 0;
ec30109b 0:d3db2b0afc76 175
ec30109b 0:d3db2b0afc76 176 motor[4].setSpeed(mecha_power[0]);
ec30109b 0:d3db2b0afc76 177 motor[5].setSpeed(mecha_power[1]);
ec30109b 0:d3db2b0afc76 178 motor[6].setSpeed(mecha_power[2]);
ec30109b 0:d3db2b0afc76 179 motor[7].setSpeed(mecha_power[3]);
ec30109b 0:d3db2b0afc76 180 }
ec30109b 0:d3db2b0afc76 181
ec30109b 0:d3db2b0afc76 182 void stop()
ec30109b 0:d3db2b0afc76 183 {
ec30109b 0:d3db2b0afc76 184 for(int i=4;i<8;i++) motor[i].setSpeed(0);
ec30109b 0:d3db2b0afc76 185 }
ec30109b 0:d3db2b0afc76 186
ec30109b 0:d3db2b0afc76 187 void Drop()
ec30109b 0:d3db2b0afc76 188 {
ec30109b 0:d3db2b0afc76 189 if(b[4] == 1 && b[5] == 0){
ec30109b 0:d3db2b0afc76 190 mecha_power[3] = 0.5;
ec30109b 0:d3db2b0afc76 191 }else if(b[4] == 0 && b[5] == 1) {
ec30109b 0:d3db2b0afc76 192 mecha_power[3] = -0.5;
ec30109b 0:d3db2b0afc76 193 }else{
ec30109b 0:d3db2b0afc76 194 mecha_power[3] = 0;
ec30109b 0:d3db2b0afc76 195 }
ec30109b 0:d3db2b0afc76 196 motor[7].setSpeed(mecha_power[3]);
ec30109b 0:d3db2b0afc76 197 }
ec30109b 0:d3db2b0afc76 198
ec30109b 0:d3db2b0afc76 199 int main()
ec30109b 0:d3db2b0afc76 200 {
ec30109b 0:d3db2b0afc76 201 sw1 = 1;
ec30109b 0:d3db2b0afc76 202 double nowX,nowY,t;
ec30109b 0:d3db2b0afc76 203
ec30109b 0:d3db2b0afc76 204 for(int i=0;i<4;i++)motor[i].braking = false;
ec30109b 0:d3db2b0afc76 205 omni.wheel[0].setRadian(PI * 1 / 4);
ec30109b 0:d3db2b0afc76 206 omni.wheel[1].setRadian(PI * 3 / 4);
ec30109b 0:d3db2b0afc76 207 omni.wheel[2].setRadian(PI * 5 / 4);
ec30109b 0:d3db2b0afc76 208 omni.wheel[3].setRadian(PI * 7 / 4);
ec30109b 0:d3db2b0afc76 209 angle.setInputLimits(-6.28,6.28);
ec30109b 0:d3db2b0afc76 210 angle.setOutputLimits(-0.4,0.4);
ec30109b 0:d3db2b0afc76 211 angle.setBias(0);
ec30109b 0:d3db2b0afc76 212 angle.setMode(1);
ec30109b 0:d3db2b0afc76 213 angle.setSetPoint(0);
ec30109b 0:d3db2b0afc76 214
ec30109b 0:d3db2b0afc76 215 button1.countreset();
ec30109b 0:d3db2b0afc76 216
ec30109b 0:d3db2b0afc76 217 sensor.startReceive(28);
ec30109b 0:d3db2b0afc76 218
ec30109b 0:d3db2b0afc76 219 while(1){
ec30109b 0:d3db2b0afc76 220 LED = !LED;
ec30109b 0:d3db2b0afc76 221
ec30109b 0:d3db2b0afc76 222 towel.setPulse(loli[2]);
ec30109b 0:d3db2b0afc76 223
piroro4560 1:e7413a7b013b 224 // PS3 value
ec30109b 0:d3db2b0afc76 225 for(int i = 0; i < 12; i++) {
ec30109b 0:d3db2b0afc76 226 b[i] = ps3.getButton(i);
ec30109b 0:d3db2b0afc76 227 pc.printf("%2d", b[i] );
ec30109b 0:d3db2b0afc76 228 }
ec30109b 0:d3db2b0afc76 229 for(int i = 0; i < 4; i++) {
ec30109b 0:d3db2b0afc76 230 stick[i] = ps3.getStick(i);
ec30109b 0:d3db2b0afc76 231 pc.printf("%4d", stick[i] );
ec30109b 0:d3db2b0afc76 232 }
ec30109b 0:d3db2b0afc76 233 X0 = stick[0];
ec30109b 0:d3db2b0afc76 234 Y0 = stick[1];
ec30109b 0:d3db2b0afc76 235 X1 = stick[2];
ec30109b 0:d3db2b0afc76 236 Y1 = stick[3];
ec30109b 0:d3db2b0afc76 237
ec30109b 0:d3db2b0afc76 238 for(int i = 0; i < 2; i++) {
ec30109b 0:d3db2b0afc76 239 trigger[i] = ps3.getTrigger(i);
ec30109b 0:d3db2b0afc76 240 // pc.printf("%4d",trigger[i]);
ec30109b 0:d3db2b0afc76 241 }
ec30109b 0:d3db2b0afc76 242
ec30109b 0:d3db2b0afc76 243 //Sensor value
ec30109b 0:d3db2b0afc76 244 getSensorData();
ec30109b 0:d3db2b0afc76 245
ec30109b 0:d3db2b0afc76 246 //Count
ec30109b 0:d3db2b0afc76 247 button1.assignvalue(b[9]);
ec30109b 0:d3db2b0afc76 248 num = button1.getCount();
ec30109b 0:d3db2b0afc76 249
ec30109b 0:d3db2b0afc76 250 /*【Action】*/
ec30109b 0:d3db2b0afc76 251 switch(num){
ec30109b 0:d3db2b0afc76 252 case 0: angle.setSetPoint(0);
ec30109b 0:d3db2b0afc76 253 clossmove0();
ec30109b 0:d3db2b0afc76 254 Recovery();
ec30109b 0:d3db2b0afc76 255 towel.open();
ec30109b 0:d3db2b0afc76 256 air.solenoid1_open();
ec30109b 0:d3db2b0afc76 257 nowX = posi.getX();
ec30109b 0:d3db2b0afc76 258 nowY = posi.getY();
ec30109b 0:d3db2b0afc76 259 break;
ec30109b 0:d3db2b0afc76 260 case 1: proto.targetXY(-3000, 0, nowX ,nowY);
ec30109b 0:d3db2b0afc76 261 towel.allstop();
ec30109b 0:d3db2b0afc76 262 stop();
ec30109b 0:d3db2b0afc76 263 break;
ec30109b 0:d3db2b0afc76 264 case 2: angle.setSetPoint(1.57);
ec30109b 0:d3db2b0afc76 265 Drop();
ec30109b 0:d3db2b0afc76 266 clossmove3();
ec30109b 0:d3db2b0afc76 267 towel.allstop();
ec30109b 0:d3db2b0afc76 268 stop();
ec30109b 0:d3db2b0afc76 269 nowX = posi.getX();
ec30109b 0:d3db2b0afc76 270 nowY = posi.getY();
ec30109b 0:d3db2b0afc76 271 break;
ec30109b 0:d3db2b0afc76 272 case 3: proto.targetXY(-2000, 3000, nowX ,nowY);
ec30109b 0:d3db2b0afc76 273 towel.allstop();
ec30109b 0:d3db2b0afc76 274 stop();
ec30109b 0:d3db2b0afc76 275 if((posi.getX()>-2200 && posi.getX()<-1800) && (posi.getY()>2800 && posi.getY()<3200)){
ec30109b 0:d3db2b0afc76 276 num = 4;
ec30109b 0:d3db2b0afc76 277 angle.setSetPoint(0);
ec30109b 0:d3db2b0afc76 278 }
ec30109b 0:d3db2b0afc76 279 break;
ec30109b 0:d3db2b0afc76 280 case 4: clossmove0();
ec30109b 0:d3db2b0afc76 281 // Recovery();
ec30109b 0:d3db2b0afc76 282 // nowX = posi.getX();
ec30109b 0:d3db2b0afc76 283 // nowY = posi.getY();
ec30109b 0:d3db2b0afc76 284 // break;
ec30109b 0:d3db2b0afc76 285 // case 5: proto.targetXY(-3000, 0, nowX ,nowY);
ec30109b 0:d3db2b0afc76 286 // break;
ec30109b 0:d3db2b0afc76 287 // case 6: angle.setSetPoint(1.57);
ec30109b 0:d3db2b0afc76 288 // Drop();
ec30109b 0:d3db2b0afc76 289 // clossmove3();
ec30109b 0:d3db2b0afc76 290 // nowX = posi.getX();
ec30109b 0:d3db2b0afc76 291 // nowY = posi.getY();
ec30109b 0:d3db2b0afc76 292 // break;
ec30109b 0:d3db2b0afc76 293 // case 7: proto.targetXY(-3000,2600,-3000,0);
ec30109b 0:d3db2b0afc76 294 // if(posi.getX()>-3200 && posi.getX()<-2800 && posi.getY()>2400 && posi.getY()<2800){
ec30109b 0:d3db2b0afc76 295 // num = 8;
ec30109b 0:d3db2b0afc76 296 // }
ec30109b 0:d3db2b0afc76 297 // break;
ec30109b 0:d3db2b0afc76 298 // case 8: angle.setSetPoint(1.57);
ec30109b 0:d3db2b0afc76 299 // clossmove3();
ec30109b 0:d3db2b0afc76 300 // nowX = posi.getX();
ec30109b 0:d3db2b0afc76 301 // nowY = posi.getY();
ec30109b 0:d3db2b0afc76 302 //
ec30109b 0:d3db2b0afc76 303 // towel.drop();
ec30109b 0:d3db2b0afc76 304 // t = timer.read();
ec30109b 0:d3db2b0afc76 305 // if(b[4] == 1 && b[5] == 1){
ec30109b 0:d3db2b0afc76 306 // timer.start();
ec30109b 0:d3db2b0afc76 307 // }
ec30109b 0:d3db2b0afc76 308 // if(t > 5){
ec30109b 0:d3db2b0afc76 309 // towel.close();
ec30109b 0:d3db2b0afc76 310 // }else if(t > 20){
ec30109b 0:d3db2b0afc76 311 // angle.setSetPoint(0);
ec30109b 0:d3db2b0afc76 312 // air.solenoid2_open();
ec30109b 0:d3db2b0afc76 313 // }
ec30109b 0:d3db2b0afc76 314 // break;
ec30109b 0:d3db2b0afc76 315 // case 9: angle.setSetPoint(0);
ec30109b 0:d3db2b0afc76 316 // proto.targetXY(0, 0,nowX ,nowY);
ec30109b 0:d3db2b0afc76 317 // break;
ec30109b 0:d3db2b0afc76 318 default: motor[0].setSpeed(0);
ec30109b 0:d3db2b0afc76 319 motor[1].setSpeed(0);
ec30109b 0:d3db2b0afc76 320 motor[2].setSpeed(0);
ec30109b 0:d3db2b0afc76 321 motor[3].setSpeed(0);
ec30109b 0:d3db2b0afc76 322
ec30109b 0:d3db2b0afc76 323 air.solenoid1_close();
ec30109b 0:d3db2b0afc76 324 air.solenoid2_close();
ec30109b 0:d3db2b0afc76 325
ec30109b 0:d3db2b0afc76 326 }
ec30109b 0:d3db2b0afc76 327
ec30109b 0:d3db2b0afc76 328 angle.setProcessValue(posi.getTheta());
ec30109b 0:d3db2b0afc76 329 spin_power = -angle.compute();
ec30109b 0:d3db2b0afc76 330
ec30109b 0:d3db2b0afc76 331 if(num == 0 || num == 2 || num == 4 || num == 6 || num == 8){
ec30109b 0:d3db2b0afc76 332 omni.computeXY(x2,y2,spin_power);
ec30109b 0:d3db2b0afc76 333 }else{
ec30109b 0:d3db2b0afc76 334 proto.Input_nowXY(posi.getX(),posi.getY());
ec30109b 0:d3db2b0afc76 335 proto.calculate();
ec30109b 0:d3db2b0afc76 336 omni.computeXY(proto.getvalue_x(), proto.getvalue_y(), spin_power);
ec30109b 0:d3db2b0afc76 337 }
piroro4560 1:e7413a7b013b 338 /*
piroro4560 1:e7413a7b013b 339 omni.conputeCircular(proto.getValue(), proto.gettheta(), spin_power);
piroro4560 1:e7413a7b013b 340 機体の向きに関係ないスティック操作をしたい場合はspin_powerに、スティックの角度（atan2(スティックのy座標,スティックのx座標)）を足せば良い
piroro4560 1:e7413a7b013b 341 */
ec30109b 0:d3db2b0afc76 342
ec30109b 0:d3db2b0afc76 343 for(int i = 0; i < 4; i++){
ec30109b 0:d3db2b0afc76 344 value[i] = omni.wheel[i];
ec30109b 0:d3db2b0afc76 345 motor[i].setSpeed(value[i]);
ec30109b 0:d3db2b0afc76 346 // pc.printf("%f ", value[i]);
ec30109b 0:d3db2b0afc76 347 }
ec30109b 0:d3db2b0afc76 348
ec30109b 0:d3db2b0afc76 349 // wait(0.001);
ec30109b 0:d3db2b0afc76 350 pc.printf(" loli:%d case:%d",loli[2],num);
ec30109b 0:d3db2b0afc76 351
ec30109b 0:d3db2b0afc76 352 //Wheel value
ec30109b 0:d3db2b0afc76 353 // pc.printf(" vector:%f pid:%.2f",proto.vector,spin_power);
ec30109b 0:d3db2b0afc76 354 //Odmetry
ec30109b 0:d3db2b0afc76 355 // pc.printf("x:%d y:%d θ:%f now:%f",posi.getX(),posi.getY(),posi.getTheta(),proto.nowDis);
ec30109b 0:d3db2b0afc76 356
ec30109b 0:d3db2b0afc76 357 pc.printf("\r\n");
piroro4560 1:e7413a7b013b 358 for(int i = 0; i < 12; i++) {
piroro4560 1:e7413a7b013b 359 b+[i] = ps3.getButton(i);
piroro4560 1:e7413a7b013b 360 }
ec30109b 0:d3db2b0afc76 361 }
ec30109b 0:d3db2b0afc76 362 }