Soma Takahashi / Mbed OS 2019NHK_A_manual_red

damedayo

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

main.cpp@0:d3db2b0afc76, 2019-09-23 (annotated)

Committer:
ec30109b
Date:
Mon Sep 23 12:25:26 2019 +0000
Revision:
0:d3db2b0afc76
madamadadame

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
ec30109b 0:d3db2b0afc76 28 int num = 0,X,Y,b[12],stick[4],trigger[2],loli[3];
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
ec30109b 0:d3db2b0afc76 53 void getSensorData() //Receive sensor value
ec30109b 0:d3db2b0afc76 54 {
ec30109b 0:d3db2b0afc76 55 UnionBytes bytedata;
ec30109b 0:d3db2b0afc76 56 sensor.getData(bytedata.bytes);
ec30109b 0:d3db2b0afc76 57 for(int i=0; i<2; i++){
ec30109b 0:d3db2b0afc76 58 IRdistance[i] = bytedata.IRdist[i];
ec30109b 0:d3db2b0afc76 59 }
ec30109b 0:d3db2b0afc76 60 for(int i=0; i<2; i++){
ec30109b 0:d3db2b0afc76 61 TFdistance[i] = bytedata.TFdist[i+2];
ec30109b 0:d3db2b0afc76 62 }
ec30109b 0:d3db2b0afc76 63 for(int i=0; i<3; i++){
ec30109b 0:d3db2b0afc76 64 loli[i] = bytedata.pulses[i+4];
ec30109b 0:d3db2b0afc76 65 }
ec30109b 0:d3db2b0afc76 66 }
ec30109b 0:d3db2b0afc76 67
ec30109b 0:d3db2b0afc76 68 void clossmove0() //Adjustment,Airfront
ec30109b 0:d3db2b0afc76 69 {
ec30109b 0:d3db2b0afc76 70 if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
ec30109b 0:d3db2b0afc76 71 x2=0;
ec30109b 0:d3db2b0afc76 72 y2=0.3;
ec30109b 0:d3db2b0afc76 73 }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
ec30109b 0:d3db2b0afc76 74 x2=0;
ec30109b 0:d3db2b0afc76 75 y2=-0.3;
ec30109b 0:d3db2b0afc76 76 }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
ec30109b 0:d3db2b0afc76 77 x2=-0.3;
ec30109b 0:d3db2b0afc76 78 y2=0;
ec30109b 0:d3db2b0afc76 79 }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
ec30109b 0:d3db2b0afc76 80 x2=0.3;
ec30109b 0:d3db2b0afc76 81 y2=0;
ec30109b 0:d3db2b0afc76 82 }else{
ec30109b 0:d3db2b0afc76 83 x2=0;
ec30109b 0:d3db2b0afc76 84 y2=0;
ec30109b 0:d3db2b0afc76 85 }
ec30109b 0:d3db2b0afc76 86 }
ec30109b 0:d3db2b0afc76 87
ec30109b 0:d3db2b0afc76 88 void clossmove1() //Adjustment,Recoveryfront
ec30109b 0:d3db2b0afc76 89 {
ec30109b 0:d3db2b0afc76 90 if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
ec30109b 0:d3db2b0afc76 91 x2=0.3;
ec30109b 0:d3db2b0afc76 92 y2=0;
ec30109b 0:d3db2b0afc76 93 }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
ec30109b 0:d3db2b0afc76 94 x2=-0.3;
ec30109b 0:d3db2b0afc76 95 y2=0;
ec30109b 0:d3db2b0afc76 96 }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
ec30109b 0:d3db2b0afc76 97 x2=0;
ec30109b 0:d3db2b0afc76 98 y2=0.3;
ec30109b 0:d3db2b0afc76 99 }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
ec30109b 0:d3db2b0afc76 100 x2=0;
ec30109b 0:d3db2b0afc76 101 y2=-0.3;
ec30109b 0:d3db2b0afc76 102 }else{
ec30109b 0:d3db2b0afc76 103 x2=0;
ec30109b 0:d3db2b0afc76 104 y2=0;
ec30109b 0:d3db2b0afc76 105 }
ec30109b 0:d3db2b0afc76 106 }
ec30109b 0:d3db2b0afc76 107
ec30109b 0:d3db2b0afc76 108 void clossmove2() //Adjustment,Dropfront
ec30109b 0:d3db2b0afc76 109 {
ec30109b 0:d3db2b0afc76 110 if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
ec30109b 0:d3db2b0afc76 111 x2=0;
ec30109b 0:d3db2b0afc76 112 y2=-0.3;
ec30109b 0:d3db2b0afc76 113 }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
ec30109b 0:d3db2b0afc76 114 x2=0;
ec30109b 0:d3db2b0afc76 115 y2=0.3;
ec30109b 0:d3db2b0afc76 116 }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
ec30109b 0:d3db2b0afc76 117 x2=0.3;
ec30109b 0:d3db2b0afc76 118 y2=0;
ec30109b 0:d3db2b0afc76 119 }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
ec30109b 0:d3db2b0afc76 120 x2=-0.3;
ec30109b 0:d3db2b0afc76 121 y2=0;
ec30109b 0:d3db2b0afc76 122 }else{
ec30109b 0:d3db2b0afc76 123 x2=0;
ec30109b 0:d3db2b0afc76 124 y2=0;
ec30109b 0:d3db2b0afc76 125 }
ec30109b 0:d3db2b0afc76 126 }
ec30109b 0:d3db2b0afc76 127
ec30109b 0:d3db2b0afc76 128 void clossmove3() //Adjustment,Towelfront
ec30109b 0:d3db2b0afc76 129 {
ec30109b 0:d3db2b0afc76 130 if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
ec30109b 0:d3db2b0afc76 131 x2=-0.3;
ec30109b 0:d3db2b0afc76 132 y2=0;
ec30109b 0:d3db2b0afc76 133 }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
ec30109b 0:d3db2b0afc76 134 x2=0.3;
ec30109b 0:d3db2b0afc76 135 y2=0;
ec30109b 0:d3db2b0afc76 136 }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
ec30109b 0:d3db2b0afc76 137 x2=0;
ec30109b 0:d3db2b0afc76 138 y2=-0.3;
ec30109b 0:d3db2b0afc76 139 }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
ec30109b 0:d3db2b0afc76 140 x2=0;
ec30109b 0:d3db2b0afc76 141 y2=0.3;
ec30109b 0:d3db2b0afc76 142 }else{
ec30109b 0:d3db2b0afc76 143 x2=0;
ec30109b 0:d3db2b0afc76 144 y2=0;
ec30109b 0:d3db2b0afc76 145 }
ec30109b 0:d3db2b0afc76 146 }
ec30109b 0:d3db2b0afc76 147
ec30109b 0:d3db2b0afc76 148 void Recovery() //Recovery mechanism
ec30109b 0:d3db2b0afc76 149 {
ec30109b 0:d3db2b0afc76 150 /* 洗濯物を挟む機構 */
ec30109b 0:d3db2b0afc76 151 if((X0 > 170)&&(Y0 > 50 && Y0 < 200)) {
ec30109b 0:d3db2b0afc76 152 mecha_power[1] = 0.6;
ec30109b 0:d3db2b0afc76 153 }else if((X0 < 100)&&(Y0 > 50 && Y0 < 200)) {
ec30109b 0:d3db2b0afc76 154 mecha_power[1] = -0.6;
ec30109b 0:d3db2b0afc76 155 }else{
ec30109b 0:d3db2b0afc76 156 mecha_power[1] = 0;
ec30109b 0:d3db2b0afc76 157 }
ec30109b 0:d3db2b0afc76 158 if((X1 > 170)&&(Y1 > 50 && Y1 < 200)) {
ec30109b 0:d3db2b0afc76 159 mecha_power[2] = 0.6;
ec30109b 0:d3db2b0afc76 160 }else if((X1 < 100)&&(Y1 > 50 && Y1 < 200)) {
ec30109b 0:d3db2b0afc76 161 mecha_power[2] = -0.6;
ec30109b 0:d3db2b0afc76 162 }else{
ec30109b 0:d3db2b0afc76 163 mecha_power[2] = 0;
ec30109b 0:d3db2b0afc76 164 }
ec30109b 0:d3db2b0afc76 165 /* 洗濯物を挟む機構の首 */
ec30109b 0:d3db2b0afc76 166 if (b[10] == 1 && b[11] == 0) {
ec30109b 0:d3db2b0afc76 167 mecha_power[0] = 0.3;
ec30109b 0:d3db2b0afc76 168 } else if (b[10] == 0 && b[11] == 1) {
ec30109b 0:d3db2b0afc76 169 mecha_power[0] = -0.3;
ec30109b 0:d3db2b0afc76 170 } else {
ec30109b 0:d3db2b0afc76 171 mecha_power[0] =0.0;
ec30109b 0:d3db2b0afc76 172 }
ec30109b 0:d3db2b0afc76 173 mecha_power[3] = 0;
ec30109b 0:d3db2b0afc76 174
ec30109b 0:d3db2b0afc76 175 motor[4].setSpeed(mecha_power[0]);
ec30109b 0:d3db2b0afc76 176 motor[5].setSpeed(mecha_power[1]);
ec30109b 0:d3db2b0afc76 177 motor[6].setSpeed(mecha_power[2]);
ec30109b 0:d3db2b0afc76 178 motor[7].setSpeed(mecha_power[3]);
ec30109b 0:d3db2b0afc76 179 }
ec30109b 0:d3db2b0afc76 180
ec30109b 0:d3db2b0afc76 181 void stop()
ec30109b 0:d3db2b0afc76 182 {
ec30109b 0:d3db2b0afc76 183 for(int i=4;i<8;i++) motor[i].setSpeed(0);
ec30109b 0:d3db2b0afc76 184 }
ec30109b 0:d3db2b0afc76 185
ec30109b 0:d3db2b0afc76 186 void Drop()
ec30109b 0:d3db2b0afc76 187 {
ec30109b 0:d3db2b0afc76 188 if(b[4] == 1 && b[5] == 0){
ec30109b 0:d3db2b0afc76 189 mecha_power[3] = 0.5;
ec30109b 0:d3db2b0afc76 190 }else if(b[4] == 0 && b[5] == 1) {
ec30109b 0:d3db2b0afc76 191 mecha_power[3] = -0.5;
ec30109b 0:d3db2b0afc76 192 }else{
ec30109b 0:d3db2b0afc76 193 mecha_power[3] = 0;
ec30109b 0:d3db2b0afc76 194 }
ec30109b 0:d3db2b0afc76 195 motor[7].setSpeed(mecha_power[3]);
ec30109b 0:d3db2b0afc76 196 }
ec30109b 0:d3db2b0afc76 197
ec30109b 0:d3db2b0afc76 198 int main()
ec30109b 0:d3db2b0afc76 199 {
ec30109b 0:d3db2b0afc76 200 sw1 = 1;
ec30109b 0:d3db2b0afc76 201 double nowX,nowY,t;
ec30109b 0:d3db2b0afc76 202
ec30109b 0:d3db2b0afc76 203 for(int i=0;i<4;i++)motor[i].braking = false;
ec30109b 0:d3db2b0afc76 204 omni.wheel[0].setRadian(PI * 1 / 4);
ec30109b 0:d3db2b0afc76 205 omni.wheel[1].setRadian(PI * 3 / 4);
ec30109b 0:d3db2b0afc76 206 omni.wheel[2].setRadian(PI * 5 / 4);
ec30109b 0:d3db2b0afc76 207 omni.wheel[3].setRadian(PI * 7 / 4);
ec30109b 0:d3db2b0afc76 208 angle.setInputLimits(-6.28,6.28);
ec30109b 0:d3db2b0afc76 209 angle.setOutputLimits(-0.4,0.4);
ec30109b 0:d3db2b0afc76 210 angle.setBias(0);
ec30109b 0:d3db2b0afc76 211 angle.setMode(1);
ec30109b 0:d3db2b0afc76 212 angle.setSetPoint(0);
ec30109b 0:d3db2b0afc76 213
ec30109b 0:d3db2b0afc76 214 button1.countreset();
ec30109b 0:d3db2b0afc76 215
ec30109b 0:d3db2b0afc76 216 sensor.startReceive(28);
ec30109b 0:d3db2b0afc76 217
ec30109b 0:d3db2b0afc76 218 while(1){
ec30109b 0:d3db2b0afc76 219 LED = !LED;
ec30109b 0:d3db2b0afc76 220
ec30109b 0:d3db2b0afc76 221 towel.setPulse(loli[2]);
ec30109b 0:d3db2b0afc76 222
ec30109b 0:d3db2b0afc76 223 //PS3 value
ec30109b 0:d3db2b0afc76 224 for(int i = 0; i < 12; i++) {
ec30109b 0:d3db2b0afc76 225 b[i] = ps3.getButton(i);
ec30109b 0:d3db2b0afc76 226 pc.printf("%2d", b[i] );
ec30109b 0:d3db2b0afc76 227 }
ec30109b 0:d3db2b0afc76 228 for(int i = 0; i < 4; i++) {
ec30109b 0:d3db2b0afc76 229 stick[i] = ps3.getStick(i);
ec30109b 0:d3db2b0afc76 230 pc.printf("%4d", stick[i] );
ec30109b 0:d3db2b0afc76 231 }
ec30109b 0:d3db2b0afc76 232 X0 = stick[0];
ec30109b 0:d3db2b0afc76 233 Y0 = stick[1];
ec30109b 0:d3db2b0afc76 234 X1 = stick[2];
ec30109b 0:d3db2b0afc76 235 Y1 = stick[3];
ec30109b 0:d3db2b0afc76 236
ec30109b 0:d3db2b0afc76 237 for(int i = 0; i < 2; i++) {
ec30109b 0:d3db2b0afc76 238 trigger[i] = ps3.getTrigger(i);
ec30109b 0:d3db2b0afc76 239 // pc.printf("%4d",trigger[i]);
ec30109b 0:d3db2b0afc76 240 }
ec30109b 0:d3db2b0afc76 241
ec30109b 0:d3db2b0afc76 242 //Sensor value
ec30109b 0:d3db2b0afc76 243 getSensorData();
ec30109b 0:d3db2b0afc76 244
ec30109b 0:d3db2b0afc76 245 //Count
ec30109b 0:d3db2b0afc76 246 button1.assignvalue(b[9]);
ec30109b 0:d3db2b0afc76 247 num = button1.getCount();
ec30109b 0:d3db2b0afc76 248
ec30109b 0:d3db2b0afc76 249 /*【Action】*/
ec30109b 0:d3db2b0afc76 250 switch(num){
ec30109b 0:d3db2b0afc76 251 case 0: angle.setSetPoint(0);
ec30109b 0:d3db2b0afc76 252 clossmove0();
ec30109b 0:d3db2b0afc76 253 Recovery();
ec30109b 0:d3db2b0afc76 254 towel.open();
ec30109b 0:d3db2b0afc76 255 air.solenoid1_open();
ec30109b 0:d3db2b0afc76 256 nowX = posi.getX();
ec30109b 0:d3db2b0afc76 257 nowY = posi.getY();
ec30109b 0:d3db2b0afc76 258 break;
ec30109b 0:d3db2b0afc76 259 case 1: proto.targetXY(-3000, 0, nowX ,nowY);
ec30109b 0:d3db2b0afc76 260 towel.allstop();
ec30109b 0:d3db2b0afc76 261 stop();
ec30109b 0:d3db2b0afc76 262 break;
ec30109b 0:d3db2b0afc76 263 case 2: angle.setSetPoint(1.57);
ec30109b 0:d3db2b0afc76 264 Drop();
ec30109b 0:d3db2b0afc76 265 clossmove3();
ec30109b 0:d3db2b0afc76 266 towel.allstop();
ec30109b 0:d3db2b0afc76 267 stop();
ec30109b 0:d3db2b0afc76 268 nowX = posi.getX();
ec30109b 0:d3db2b0afc76 269 nowY = posi.getY();
ec30109b 0:d3db2b0afc76 270 break;
ec30109b 0:d3db2b0afc76 271 case 3: proto.targetXY(-2000, 3000, nowX ,nowY);
ec30109b 0:d3db2b0afc76 272 towel.allstop();
ec30109b 0:d3db2b0afc76 273 stop();
ec30109b 0:d3db2b0afc76 274 if((posi.getX()>-2200 && posi.getX()<-1800) && (posi.getY()>2800 && posi.getY()<3200)){
ec30109b 0:d3db2b0afc76 275 num = 4;
ec30109b 0:d3db2b0afc76 276 angle.setSetPoint(0);
ec30109b 0:d3db2b0afc76 277 }
ec30109b 0:d3db2b0afc76 278 break;
ec30109b 0:d3db2b0afc76 279 case 4: clossmove0();
ec30109b 0:d3db2b0afc76 280 // Recovery();
ec30109b 0:d3db2b0afc76 281 // nowX = posi.getX();
ec30109b 0:d3db2b0afc76 282 // nowY = posi.getY();
ec30109b 0:d3db2b0afc76 283 // break;
ec30109b 0:d3db2b0afc76 284 // case 5: proto.targetXY(-3000, 0, nowX ,nowY);
ec30109b 0:d3db2b0afc76 285 // break;
ec30109b 0:d3db2b0afc76 286 // case 6: angle.setSetPoint(1.57);
ec30109b 0:d3db2b0afc76 287 // Drop();
ec30109b 0:d3db2b0afc76 288 // clossmove3();
ec30109b 0:d3db2b0afc76 289 // nowX = posi.getX();
ec30109b 0:d3db2b0afc76 290 // nowY = posi.getY();
ec30109b 0:d3db2b0afc76 291 // break;
ec30109b 0:d3db2b0afc76 292 // case 7: proto.targetXY(-3000,2600,-3000,0);
ec30109b 0:d3db2b0afc76 293 // if(posi.getX()>-3200 && posi.getX()<-2800 && posi.getY()>2400 && posi.getY()<2800){
ec30109b 0:d3db2b0afc76 294 // num = 8;
ec30109b 0:d3db2b0afc76 295 // }
ec30109b 0:d3db2b0afc76 296 // break;
ec30109b 0:d3db2b0afc76 297 // case 8: angle.setSetPoint(1.57);
ec30109b 0:d3db2b0afc76 298 // clossmove3();
ec30109b 0:d3db2b0afc76 299 // nowX = posi.getX();
ec30109b 0:d3db2b0afc76 300 // nowY = posi.getY();
ec30109b 0:d3db2b0afc76 301 //
ec30109b 0:d3db2b0afc76 302 // towel.drop();
ec30109b 0:d3db2b0afc76 303 // t = timer.read();
ec30109b 0:d3db2b0afc76 304 // if(b[4] == 1 && b[5] == 1){
ec30109b 0:d3db2b0afc76 305 // timer.start();
ec30109b 0:d3db2b0afc76 306 // }
ec30109b 0:d3db2b0afc76 307 // if(t > 5){
ec30109b 0:d3db2b0afc76 308 // towel.close();
ec30109b 0:d3db2b0afc76 309 // }else if(t > 20){
ec30109b 0:d3db2b0afc76 310 // angle.setSetPoint(0);
ec30109b 0:d3db2b0afc76 311 // air.solenoid2_open();
ec30109b 0:d3db2b0afc76 312 // }
ec30109b 0:d3db2b0afc76 313 // break;
ec30109b 0:d3db2b0afc76 314 // case 9: angle.setSetPoint(0);
ec30109b 0:d3db2b0afc76 315 // proto.targetXY(0, 0,nowX ,nowY);
ec30109b 0:d3db2b0afc76 316 // break;
ec30109b 0:d3db2b0afc76 317 default: motor[0].setSpeed(0);
ec30109b 0:d3db2b0afc76 318 motor[1].setSpeed(0);
ec30109b 0:d3db2b0afc76 319 motor[2].setSpeed(0);
ec30109b 0:d3db2b0afc76 320 motor[3].setSpeed(0);
ec30109b 0:d3db2b0afc76 321
ec30109b 0:d3db2b0afc76 322 air.solenoid1_close();
ec30109b 0:d3db2b0afc76 323 air.solenoid2_close();
ec30109b 0:d3db2b0afc76 324
ec30109b 0:d3db2b0afc76 325 }
ec30109b 0:d3db2b0afc76 326
ec30109b 0:d3db2b0afc76 327 angle.setProcessValue(posi.getTheta());
ec30109b 0:d3db2b0afc76 328 spin_power = -angle.compute();
ec30109b 0:d3db2b0afc76 329
ec30109b 0:d3db2b0afc76 330 if(num == 0 || num == 2 || num == 4 || num == 6 || num == 8){
ec30109b 0:d3db2b0afc76 331 omni.computeXY(x2,y2,spin_power);
ec30109b 0:d3db2b0afc76 332 }else{
ec30109b 0:d3db2b0afc76 333 proto.Input_nowXY(posi.getX(),posi.getY());
ec30109b 0:d3db2b0afc76 334 proto.calculate();
ec30109b 0:d3db2b0afc76 335 omni.computeXY(proto.getvalue_x(), proto.getvalue_y(), spin_power);
ec30109b 0:d3db2b0afc76 336 }
ec30109b 0:d3db2b0afc76 337
ec30109b 0:d3db2b0afc76 338 for(int i = 0; i < 4; i++){
ec30109b 0:d3db2b0afc76 339 value[i] = omni.wheel[i];
ec30109b 0:d3db2b0afc76 340 motor[i].setSpeed(value[i]);
ec30109b 0:d3db2b0afc76 341 // pc.printf("%f ", value[i]);
ec30109b 0:d3db2b0afc76 342 }
ec30109b 0:d3db2b0afc76 343
ec30109b 0:d3db2b0afc76 344 // wait(0.001);
ec30109b 0:d3db2b0afc76 345 pc.printf(" loli:%d case:%d",loli[2],num);
ec30109b 0:d3db2b0afc76 346
ec30109b 0:d3db2b0afc76 347 //Wheel value
ec30109b 0:d3db2b0afc76 348 // pc.printf(" vector:%f pid:%.2f",proto.vector,spin_power);
ec30109b 0:d3db2b0afc76 349 //Odmetry
ec30109b 0:d3db2b0afc76 350 // pc.printf("x:%d y:%d θ:%f now:%f",posi.getX(),posi.getY(),posi.getTheta(),proto.nowDis);
ec30109b 0:d3db2b0afc76 351
ec30109b 0:d3db2b0afc76 352 pc.printf("\r\n");
ec30109b 0:d3db2b0afc76 353 }
ec30109b 0:d3db2b0afc76 354 }