naoki tanabe / Mbed OS 2018NHK_gakugaku_robo

前のやつです。

Dependencies:   FEP OmniPosition PID QEI R1307 TFmini ikarashiMDC linesSnsor omni_wheel

gakuBot/gakubot.cpp@3:8f4c81ad256a, 2018-09-11 (annotated)

Committer:
tanabe2000
Date:
Tue Sep 11 15:05:51 2018 +0000
Revision:
3:8f4c81ad256a
Parent:
1:af8bee219a3a
Child:
5:c7643ae5835f
?????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tanabe2000 1:af8bee219a3a 1 #include "gakubot.h"
tanabe2000 1:af8bee219a3a 2
tanabe2000 1:af8bee219a3a 3 GakuBot::GakuBot() :
tanabe2000 1:af8bee219a3a 4 led(LED2),
tanabe2000 1:af8bee219a3a 5 pid(KP,KI,KD,RATE),
tanabe2000 3:8f4c81ad256a 6 anglePID(angleKP, angleKI, angleKD, RATE),
tanabe2000 1:af8bee219a3a 7 omni(4),
tanabe2000 3:8f4c81ad256a 8 angle1_1(air1_0),
tanabe2000 3:8f4c81ad256a 9 angle1_2(air1_1),
tanabe2000 1:af8bee219a3a 10 angle2_1(air2_0),angle2_2(air2_1),
tanabe2000 1:af8bee219a3a 11 con(XBee2TX, XBee2RX, ADDR),
tanabe2000 1:af8bee219a3a 12 wheel1 (PA_11,PA_12,NC, 2048, QEI::X4_ENCODING),
tanabe2000 3:8f4c81ad256a 13 // r1370(R1370TX, R1370RX),
tanabe2000 1:af8bee219a3a 14 RS485control(PA_4),
tanabe2000 3:8f4c81ad256a 15 RS485(MDTX,MDRX,115200),
tanabe2000 1:af8bee219a3a 16 debugpc(USBTX,USBRX,115200),
tanabe2000 3:8f4c81ad256a 17 receiveSuccessed(0)
tanabe2000 3:8f4c81ad256a 18
tanabe2000 1:af8bee219a3a 19 {
tanabe2000 3:8f4c81ad256a 20 for(int i = 0; i < 4; i++) stick[i] = 0;
tanabe2000 3:8f4c81ad256a 21 for(int i = 0; i < 3; i++) speed[i] = 0, firePwm[i] = 0;
tanabe2000 3:8f4c81ad256a 22 // for(int i = 0; i < 5; i++) loadingPwm[i] = 0;
tanabe2000 3:8f4c81ad256a 23 Output_PID = 0;
tanabe2000 3:8f4c81ad256a 24 nowAngle = 0;
tanabe2000 3:8f4c81ad256a 25 ofsetNowAngle = 0;
tanabe2000 3:8f4c81ad256a 26 airFlag = 0;
tanabe2000 3:8f4c81ad256a 27 airFlag2 = 0;
tanabe2000 3:8f4c81ad256a 28 airStatus = 0;
tanabe2000 3:8f4c81ad256a 29 airStatus2 = 0;
tanabe2000 3:8f4c81ad256a 30 distance = 800;
tanabe2000 3:8f4c81ad256a 31 distanceOfset = 0;
tanabe2000 3:8f4c81ad256a 32 nowPals = 0;
tanabe2000 3:8f4c81ad256a 33 mode = 0;
tanabe2000 3:8f4c81ad256a 34 demoX = 0;
tanabe2000 3:8f4c81ad256a 35 demoY = 0;
tanabe2000 3:8f4c81ad256a 36 dt = 0;
tanabe2000 3:8f4c81ad256a 37 attachAngle = 0;
tanabe2000 3:8f4c81ad256a 38 wheels[0] = new ikarashiMDC(&RS485control,0,0,SM,&RS485);
tanabe2000 3:8f4c81ad256a 39 wheels[1] = new ikarashiMDC(&RS485control,0,1,SM,&RS485);
tanabe2000 3:8f4c81ad256a 40 wheels[2] = new ikarashiMDC(&RS485control,0,2,SM,&RS485);
tanabe2000 3:8f4c81ad256a 41 wheels[3] = new ikarashiMDC(&RS485control,0,3,SM,&RS485);
tanabe2000 3:8f4c81ad256a 42
tanabe2000 3:8f4c81ad256a 43 fire[0] = new ikarashiMDC(&RS485control,1,0,SM,&RS485);
tanabe2000 3:8f4c81ad256a 44 fire[1] = new ikarashiMDC(&RS485control,1,1,SM,&RS485);
tanabe2000 3:8f4c81ad256a 45 // fire[2] = new ikarashiMDC(&RS485control,1,2,SM,&RS485);
tanabe2000 3:8f4c81ad256a 46
tanabe2000 3:8f4c81ad256a 47 // Loading[0] = new ikarashiMDC(&RS485control,2,0,SM,&RS485);
tanabe2000 3:8f4c81ad256a 48 // Loading[1] = new ikarashiMDC(&RS485control,2,1,SM,&RS485);
tanabe2000 3:8f4c81ad256a 49 // Loading[2] = new ikarashiMDC(&RS485control,2,2,SM,&RS485);
tanabe2000 3:8f4c81ad256a 50 // Loading[3] = new ikarashiMDC(&RS485control,2,3,SM,&RS485);
tanabe2000 3:8f4c81ad256a 51 // Loading[4] = new ikarashiMDC(&RS485control,1,3,SM,&RS485);
tanabe2000 3:8f4c81ad256a 52
tanabe2000 3:8f4c81ad256a 53
tanabe2000 1:af8bee219a3a 54 omni.wheel[0].setRadian(PI / 4.0 * 1.0);
tanabe2000 1:af8bee219a3a 55 omni.wheel[1].setRadian(PI / 4.0 * 3.0);
tanabe2000 1:af8bee219a3a 56 omni.wheel[2].setRadian(PI / 4.0 * 5.0);
tanabe2000 1:af8bee219a3a 57 omni.wheel[3].setRadian(PI / 4.0 * 7.0);
tanabe2000 1:af8bee219a3a 58 for(int i = 0; i < 4; i++) {
tanabe2000 3:8f4c81ad256a 59 wheels[i]->braking = true;
tanabe2000 1:af8bee219a3a 60 }
tanabe2000 3:8f4c81ad256a 61 for(int i = 0; i < 2; i++) {
tanabe2000 3:8f4c81ad256a 62 fire[i]->braking = true;
tanabe2000 1:af8bee219a3a 63 }
tanabe2000 3:8f4c81ad256a 64 //for(int i = 0; i < 5; i++) {
tanabe2000 3:8f4c81ad256a 65 // Loading[i]->braking = true;
tanabe2000 3:8f4c81ad256a 66 // }
tanabe2000 1:af8bee219a3a 67 pid.setMode(AUTO_MODE);
tanabe2000 3:8f4c81ad256a 68 pid.setInputLimits(-100, 2300);
tanabe2000 1:af8bee219a3a 69 pid.setOutputLimits(-1.0, 1.0);
tanabe2000 3:8f4c81ad256a 70 pid.setSetPoint(distance);// 目標値
tanabe2000 1:af8bee219a3a 71 pid.setBias(0.0); // outputの変わり目
tanabe2000 3:8f4c81ad256a 72
tanabe2000 3:8f4c81ad256a 73 anglePID.setMode(AUTO_MODE);
tanabe2000 3:8f4c81ad256a 74 anglePID.setInputLimits(-360, 360);
tanabe2000 3:8f4c81ad256a 75 anglePID.setOutputLimits(-1.0, 1.0);
tanabe2000 3:8f4c81ad256a 76 anglePID.setMode(AUTO_MODE);
tanabe2000 3:8f4c81ad256a 77 anglePID.setSetPoint(0.0);
tanabe2000 3:8f4c81ad256a 78 anglePID.setBias(0.0);
tanabe2000 3:8f4c81ad256a 79 t.start();
tanabe2000 3:8f4c81ad256a 80 confirmT.start();
tanabe2000 1:af8bee219a3a 81 }
tanabe2000 1:af8bee219a3a 82
tanabe2000 1:af8bee219a3a 83 void GakuBot::botConfirm()
tanabe2000 1:af8bee219a3a 84 {
tanabe2000 3:8f4c81ad256a 85 if(con.getButton2(2)==0) distance += 10;
tanabe2000 3:8f4c81ad256a 86 if(con.getButton2(3)==0) distance -= 10;
tanabe2000 3:8f4c81ad256a 87 receiveSuccessed = con.receiveState();
tanabe2000 3:8f4c81ad256a 88 nowPals = wheel1.getPulses();
tanabe2000 3:8f4c81ad256a 89 pid.setSetPoint(distance);// 目標値
tanabe2000 3:8f4c81ad256a 90 pid.setProcessValue(nowPals - distanceOfset);
tanabe2000 3:8f4c81ad256a 91 Output_PID = -1*pid.compute();
tanabe2000 3:8f4c81ad256a 92 //
tanabe2000 3:8f4c81ad256a 93 // //r1370.update();
tanabe2000 3:8f4c81ad256a 94 //// nowAngle = r1370.getRate();
tanabe2000 3:8f4c81ad256a 95 // nowAngle = 0.0;
tanabe2000 3:8f4c81ad256a 96 // anglePID.setSetPoint(attachAngle);
tanabe2000 3:8f4c81ad256a 97 // anglePID.setProcessValue(nowAngle - ofsetNowAngle);
tanabe2000 1:af8bee219a3a 98
tanabe2000 1:af8bee219a3a 99 }
tanabe2000 1:af8bee219a3a 100
tanabe2000 3:8f4c81ad256a 101 //void GakuBot::controllerMode1()
tanabe2000 3:8f4c81ad256a 102 //{
tanabe2000 3:8f4c81ad256a 103 // if(receiveSuccessed) {
tanabe2000 3:8f4c81ad256a 104 //// for(int i = 0; i < 3; i++) fire[i]->setSpeed(0);
tanabe2000 3:8f4c81ad256a 105 // for(int i = 0; i < 4; i++) stick[i] = con.getStick(i);
tanabe2000 3:8f4c81ad256a 106 //
tanabe2000 3:8f4c81ad256a 107 // if((-0.1 < stick[2] )&& (stick[2] < 0.1)) omni.computeXY(stick[0]/3,stick[1]/3, -1*anglePID.compute());
tanabe2000 3:8f4c81ad256a 108 // else {
tanabe2000 3:8f4c81ad256a 109 // omni.computeXY(stick[0]/3,stick[1]/3, -1*stick[2]/4.0);
tanabe2000 3:8f4c81ad256a 110 // ofsetNowAngle = nowAngle;
tanabe2000 3:8f4c81ad256a 111 // attachAngle = 0;
tanabe2000 3:8f4c81ad256a 112 // }
tanabe2000 3:8f4c81ad256a 113 //
tanabe2000 3:8f4c81ad256a 114 //
tanabe2000 3:8f4c81ad256a 115 // confirmDt = confirmT.read();
tanabe2000 3:8f4c81ad256a 116 // if((con.getButton1(0)==0) && (confirmDt >= 0.5))attachAngle += 10, confirmT.reset();
tanabe2000 3:8f4c81ad256a 117 // if((con.getButton1(1)==0) && (confirmDt >= 0.5))attachAngle -= 10, confirmT.reset();
tanabe2000 3:8f4c81ad256a 118 // debugpc.printf("attach = %f\r\n", attachAngle);
tanabe2000 3:8f4c81ad256a 119 //
tanabe2000 3:8f4c81ad256a 120 // for (int i = 0; i < 4; i++) {
tanabe2000 3:8f4c81ad256a 121 // speed[i] = omni.wheel[i];
tanabe2000 3:8f4c81ad256a 122 // wheels[i]->setSpeed(speed[i]);
tanabe2000 3:8f4c81ad256a 123 //// pc.printf("%1d: %.3f, ", i, speed[i]);
tanabe2000 3:8f4c81ad256a 124 // }
tanabe2000 3:8f4c81ad256a 125 // } else {
tanabe2000 3:8f4c81ad256a 126 //
tanabe2000 3:8f4c81ad256a 127 // for (int i = 0; i < 4; i++) {
tanabe2000 3:8f4c81ad256a 128 // wheels[i]->setSpeed(0);
tanabe2000 3:8f4c81ad256a 129 // }
tanabe2000 3:8f4c81ad256a 130 // }
tanabe2000 3:8f4c81ad256a 131 //}
tanabe2000 3:8f4c81ad256a 132
tanabe2000 3:8f4c81ad256a 133 void GakuBot::controllerMode2()
tanabe2000 3:8f4c81ad256a 134 {
tanabe2000 3:8f4c81ad256a 135 if(receiveSuccessed) {
tanabe2000 3:8f4c81ad256a 136 // for(int i = 0; i < 3; i++) fire[i]->setSpeed(0);
tanabe2000 3:8f4c81ad256a 137 for(int i = 0; i < 4; i++) stick[i] = con.getStick(i);
tanabe2000 3:8f4c81ad256a 138 omni.computeXY(stick[0]/3.0 ,stick[1]/3.0 , -1*stick[2]/5.0);
tanabe2000 3:8f4c81ad256a 139
tanabe2000 3:8f4c81ad256a 140 for (int i = 0; i < 4; i++) {
tanabe2000 3:8f4c81ad256a 141 speed[i] = omni.wheel[i];
tanabe2000 3:8f4c81ad256a 142 wheels[i]->setSpeed(speed[i]);
tanabe2000 3:8f4c81ad256a 143 // debugpc.printf("%1d: %.3f, ", i, speed[i]);
tanabe2000 3:8f4c81ad256a 144 }
tanabe2000 3:8f4c81ad256a 145 } else {
tanabe2000 3:8f4c81ad256a 146 debugpc.printf("error\r\n");
tanabe2000 3:8f4c81ad256a 147 for (int i = 0; i < 4; i++) {
tanabe2000 3:8f4c81ad256a 148 wheels[i]->setSpeed(0);
tanabe2000 3:8f4c81ad256a 149 }
tanabe2000 3:8f4c81ad256a 150 }
tanabe2000 3:8f4c81ad256a 151 }
tanabe2000 1:af8bee219a3a 152
tanabe2000 1:af8bee219a3a 153 void GakuBot::controllMech()
tanabe2000 1:af8bee219a3a 154 {
tanabe2000 3:8f4c81ad256a 155 if(receiveSuccessed) {
tanabe2000 3:8f4c81ad256a 156 debugpc.printf("pals = %d,distance = %d, distanceOfset = %d", nowPals - distanceOfset,distance,distanceOfset);
tanabe2000 3:8f4c81ad256a 157 if((con.getButton1(5)==0)&&(airFlag == 0)) {
tanabe2000 1:af8bee219a3a 158
tanabe2000 3:8f4c81ad256a 159 if(airStatus==1) {
tanabe2000 3:8f4c81ad256a 160 angle1_1=0;
tanabe2000 3:8f4c81ad256a 161 angle1_2=1;
tanabe2000 3:8f4c81ad256a 162 airFlag=1;
tanabe2000 3:8f4c81ad256a 163 airStatus=0;
tanabe2000 3:8f4c81ad256a 164
tanabe2000 3:8f4c81ad256a 165 led = 0;
tanabe2000 3:8f4c81ad256a 166 // wait(0.01);
tanabe2000 3:8f4c81ad256a 167 } else if(airStatus==0) {
tanabe2000 3:8f4c81ad256a 168 angle1_1=1;
tanabe2000 3:8f4c81ad256a 169 angle1_2=0;
tanabe2000 3:8f4c81ad256a 170 airFlag=1;
tanabe2000 3:8f4c81ad256a 171 airStatus=1;
tanabe2000 3:8f4c81ad256a 172 led = 1;
tanabe2000 3:8f4c81ad256a 173 distanceOfset = nowPals;
tanabe2000 3:8f4c81ad256a 174 // wait(0.01);
tanabe2000 3:8f4c81ad256a 175 }
tanabe2000 3:8f4c81ad256a 176
tanabe2000 3:8f4c81ad256a 177 // fire[0]->setSpeed(0.0);
tanabe2000 3:8f4c81ad256a 178 } else {
tanabe2000 3:8f4c81ad256a 179 airFlag=0;
tanabe2000 3:8f4c81ad256a 180 angle1_1=0;
tanabe2000 3:8f4c81ad256a 181 angle1_2=0;
tanabe2000 3:8f4c81ad256a 182 // led = 0;
tanabe2000 3:8f4c81ad256a 183 if(airStatus == 1) {
tanabe2000 3:8f4c81ad256a 184 if(con.getButton1(3)==0) {
tanabe2000 3:8f4c81ad256a 185 // distance = 970;
tanabe2000 3:8f4c81ad256a 186 firePwm[0] = Output_PID;
tanabe2000 3:8f4c81ad256a 187 } else {
tanabe2000 3:8f4c81ad256a 188 // distance = nowPals;
tanabe2000 3:8f4c81ad256a 189 if(con.getButton1(2)==0) firePwm[0] = 0.9;
tanabe2000 3:8f4c81ad256a 190 if(con.getButton1(6)==0) firePwm[0] = -0.9;
tanabe2000 3:8f4c81ad256a 191 if(con.getButton1(2) && con.getButton1(6)) firePwm[0] = 0.0;
tanabe2000 3:8f4c81ad256a 192
tanabe2000 3:8f4c81ad256a 193 }
tanabe2000 3:8f4c81ad256a 194
tanabe2000 3:8f4c81ad256a 195 }
tanabe2000 3:8f4c81ad256a 196 //fire[0]->setSpeed(Output_PID);
tanabe2000 3:8f4c81ad256a 197 // fire[0]->setSpeed(0.0);
tanabe2000 3:8f4c81ad256a 198 // if(con.getButton1(2) == 1 && con.getButton1(6) == 1) fire[0]->setSpeed(0.0);
tanabe2000 3:8f4c81ad256a 199 // if(con.getButton1(0)==0) loadingPwm[0] = 0.5, loadingPwm[1] = -0.5;
tanabe2000 3:8f4c81ad256a 200 // if(con.getButton1(1)==0) loadingPwm[0] = -0.5, loadingPwm[1] = 0.5;
tanabe2000 3:8f4c81ad256a 201 // if(con.getButton1(0) && con.getButton1(1)) loadingPwm[0] = 0.0, loadingPwm[1] = 0.0;
tanabe2000 3:8f4c81ad256a 202
tanabe2000 3:8f4c81ad256a 203 // for(int i = 0; i < 5; i++) Loading[i]->setSpeed(loadingPwm[i]);
tanabe2000 3:8f4c81ad256a 204 for(int i = 0; i < 2; i++) fire[i]->setSpeed(firePwm[i]);/*変数を作って変更する */
tanabe2000 3:8f4c81ad256a 205 }
tanabe2000 3:8f4c81ad256a 206 debugpc.printf("pid: %f\r\n", Output_PID);
tanabe2000 3:8f4c81ad256a 207 } else {
tanabe2000 3:8f4c81ad256a 208 for(int i = 0; i < 0; i++) fire[i]->setSpeed(0.0);
tanabe2000 3:8f4c81ad256a 209 // for(int i = 0; i < 5; i++) Loading[i]->setSpeed(0.0);
tanabe2000 3:8f4c81ad256a 210 }
tanabe2000 1:af8bee219a3a 211 }
tanabe2000 1:af8bee219a3a 212
tanabe2000 3:8f4c81ad256a 213 //void GakuBot::autoMode1()
tanabe2000 3:8f4c81ad256a 214 //{
tanabe2000 3:8f4c81ad256a 215 //
tanabe2000 3:8f4c81ad256a 216 // for(int i = 0; i < 3; i++) fire[i]->setSpeed(0);
tanabe2000 3:8f4c81ad256a 217 //
tanabe2000 3:8f4c81ad256a 218 // switch(mode) {/*modeをテーブルごとに分ける。example > modeT1, modeT2, modeT3, modeT4*/
tanabe2000 3:8f4c81ad256a 219 // case 0:
tanabe2000 3:8f4c81ad256a 220 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 221 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 222 // break;
tanabe2000 3:8f4c81ad256a 223 // case 1:
tanabe2000 3:8f4c81ad256a 224 // demoX = -0.5;
tanabe2000 3:8f4c81ad256a 225 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 226 // break;
tanabe2000 3:8f4c81ad256a 227 // case 2:
tanabe2000 3:8f4c81ad256a 228 // /*line収束処理*/
tanabe2000 3:8f4c81ad256a 229 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 230 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 231 //
tanabe2000 3:8f4c81ad256a 232 // break;
tanabe2000 3:8f4c81ad256a 233 // case 3:
tanabe2000 3:8f4c81ad256a 234 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 235 // demoY = 0.5;
tanabe2000 3:8f4c81ad256a 236 //
tanabe2000 3:8f4c81ad256a 237 // break;
tanabe2000 3:8f4c81ad256a 238 // case 4:
tanabe2000 3:8f4c81ad256a 239 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 240 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 241 // break;
tanabe2000 3:8f4c81ad256a 242 // case 5:
tanabe2000 3:8f4c81ad256a 243 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 244 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 245 // angle1_1=1;
tanabe2000 3:8f4c81ad256a 246 // angle1_2=0;
tanabe2000 3:8f4c81ad256a 247 // break;
tanabe2000 3:8f4c81ad256a 248 // case 6:
tanabe2000 3:8f4c81ad256a 249 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 250 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 251 // angle1_1=0;
tanabe2000 3:8f4c81ad256a 252 // angle1_2=0;
tanabe2000 3:8f4c81ad256a 253 // fire[0]->setSpeed(Output_PID);
tanabe2000 3:8f4c81ad256a 254 // break;
tanabe2000 3:8f4c81ad256a 255 // case 7:
tanabe2000 3:8f4c81ad256a 256 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 257 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 258 // angle1_1=0;
tanabe2000 3:8f4c81ad256a 259 // angle1_2=1;
tanabe2000 3:8f4c81ad256a 260 // break;
tanabe2000 3:8f4c81ad256a 261 // case 8:
tanabe2000 3:8f4c81ad256a 262 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 263 // demoY = -0.5;
tanabe2000 3:8f4c81ad256a 264 // angle1_1=0;
tanabe2000 3:8f4c81ad256a 265 // angle1_2=0;
tanabe2000 3:8f4c81ad256a 266 // break;
tanabe2000 3:8f4c81ad256a 267 // case 9:
tanabe2000 3:8f4c81ad256a 268 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 269 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 270 // break;
tanabe2000 3:8f4c81ad256a 271 // case 10:
tanabe2000 3:8f4c81ad256a 272 // demoX = 0.5;
tanabe2000 3:8f4c81ad256a 273 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 274 // break;
tanabe2000 3:8f4c81ad256a 275 // case 11:
tanabe2000 3:8f4c81ad256a 276 // /*line収束処理*/
tanabe2000 3:8f4c81ad256a 277 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 278 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 279 //
tanabe2000 3:8f4c81ad256a 280 // break;
tanabe2000 3:8f4c81ad256a 281 // case 12:
tanabe2000 3:8f4c81ad256a 282 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 283 // demoY = 0.5;
tanabe2000 3:8f4c81ad256a 284 //
tanabe2000 3:8f4c81ad256a 285 // break;
tanabe2000 3:8f4c81ad256a 286 // case 13:
tanabe2000 3:8f4c81ad256a 287 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 288 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 289 // break;
tanabe2000 3:8f4c81ad256a 290 // case 14:
tanabe2000 3:8f4c81ad256a 291 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 292 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 293 // angle1_1=1;
tanabe2000 3:8f4c81ad256a 294 // angle1_2=0;
tanabe2000 3:8f4c81ad256a 295 // break;
tanabe2000 3:8f4c81ad256a 296 // case 15:
tanabe2000 3:8f4c81ad256a 297 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 298 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 299 // angle1_1=0;
tanabe2000 3:8f4c81ad256a 300 // angle1_2=0;
tanabe2000 3:8f4c81ad256a 301 // fire[0]->setSpeed(Output_PID);
tanabe2000 3:8f4c81ad256a 302 // break;
tanabe2000 3:8f4c81ad256a 303 // case 16:
tanabe2000 3:8f4c81ad256a 304 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 305 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 306 // angle1_1=0;
tanabe2000 3:8f4c81ad256a 307 // angle1_2=1;
tanabe2000 3:8f4c81ad256a 308 // break;
tanabe2000 3:8f4c81ad256a 309 // case 17:
tanabe2000 3:8f4c81ad256a 310 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 311 // demoY = -0.5;
tanabe2000 3:8f4c81ad256a 312 // angle1_1=0;
tanabe2000 3:8f4c81ad256a 313 // angle1_2=0;
tanabe2000 3:8f4c81ad256a 314 // break;
tanabe2000 3:8f4c81ad256a 315 // case 18:
tanabe2000 3:8f4c81ad256a 316 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 317 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 318 // break;
tanabe2000 3:8f4c81ad256a 319 // case 19:
tanabe2000 3:8f4c81ad256a 320 // demoX = 0.5;
tanabe2000 3:8f4c81ad256a 321 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 322 // break;
tanabe2000 3:8f4c81ad256a 323 // case 20:
tanabe2000 3:8f4c81ad256a 324 // /*line収束処理*/
tanabe2000 3:8f4c81ad256a 325 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 326 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 327 //
tanabe2000 3:8f4c81ad256a 328 // break;
tanabe2000 3:8f4c81ad256a 329 // case 21:
tanabe2000 3:8f4c81ad256a 330 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 331 // demoY = 0.5;
tanabe2000 3:8f4c81ad256a 332 //
tanabe2000 3:8f4c81ad256a 333 // break;
tanabe2000 3:8f4c81ad256a 334 // case 22:
tanabe2000 3:8f4c81ad256a 335 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 336 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 337 // break;
tanabe2000 3:8f4c81ad256a 338 // case 23:
tanabe2000 3:8f4c81ad256a 339 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 340 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 341 // angle1_1=1;
tanabe2000 3:8f4c81ad256a 342 // angle1_2=0;
tanabe2000 3:8f4c81ad256a 343 // break;
tanabe2000 3:8f4c81ad256a 344 // case 24:
tanabe2000 3:8f4c81ad256a 345 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 346 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 347 // angle1_1=0;
tanabe2000 3:8f4c81ad256a 348 // angle1_2=0;
tanabe2000 3:8f4c81ad256a 349 // fire[0]->setSpeed(Output_PID);
tanabe2000 3:8f4c81ad256a 350 // break;
tanabe2000 3:8f4c81ad256a 351 // case 25:
tanabe2000 3:8f4c81ad256a 352 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 353 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 354 // angle1_1=0;
tanabe2000 3:8f4c81ad256a 355 // angle1_2=1;
tanabe2000 3:8f4c81ad256a 356 // break;
tanabe2000 3:8f4c81ad256a 357 // case 26:
tanabe2000 3:8f4c81ad256a 358 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 359 // demoY = -0.5;
tanabe2000 3:8f4c81ad256a 360 // angle1_1=0;
tanabe2000 3:8f4c81ad256a 361 // angle1_2=0;
tanabe2000 3:8f4c81ad256a 362 // break;
tanabe2000 3:8f4c81ad256a 363 // case 27:
tanabe2000 3:8f4c81ad256a 364 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 365 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 366 // break;
tanabe2000 3:8f4c81ad256a 367 // case 28:
tanabe2000 3:8f4c81ad256a 368 // demoX = 0.5;
tanabe2000 3:8f4c81ad256a 369 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 370 // break;
tanabe2000 3:8f4c81ad256a 371 // case 29:
tanabe2000 3:8f4c81ad256a 372 // /*line収束処理*/
tanabe2000 3:8f4c81ad256a 373 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 374 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 375 //
tanabe2000 3:8f4c81ad256a 376 // break;
tanabe2000 3:8f4c81ad256a 377 // case 30:
tanabe2000 3:8f4c81ad256a 378 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 379 // demoY = 0.5;
tanabe2000 3:8f4c81ad256a 380 //
tanabe2000 3:8f4c81ad256a 381 // break;
tanabe2000 3:8f4c81ad256a 382 // case 31:
tanabe2000 3:8f4c81ad256a 383 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 384 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 385 // break;
tanabe2000 3:8f4c81ad256a 386 // case 32:
tanabe2000 3:8f4c81ad256a 387 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 388 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 389 // angle1_1=1;
tanabe2000 3:8f4c81ad256a 390 // angle1_2=0;
tanabe2000 3:8f4c81ad256a 391 // break;
tanabe2000 3:8f4c81ad256a 392 // case 33:
tanabe2000 3:8f4c81ad256a 393 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 394 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 395 // angle1_1=0;
tanabe2000 3:8f4c81ad256a 396 // angle1_2=0;
tanabe2000 3:8f4c81ad256a 397 // fire[0]->setSpeed(Output_PID);
tanabe2000 3:8f4c81ad256a 398 // break;
tanabe2000 3:8f4c81ad256a 399 // case 34:
tanabe2000 3:8f4c81ad256a 400 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 401 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 402 // angle1_1=0;
tanabe2000 3:8f4c81ad256a 403 // angle1_2=1;
tanabe2000 3:8f4c81ad256a 404 // break;
tanabe2000 3:8f4c81ad256a 405 // case 35:
tanabe2000 3:8f4c81ad256a 406 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 407 // demoY = -0.5;
tanabe2000 3:8f4c81ad256a 408 // angle1_1=0;
tanabe2000 3:8f4c81ad256a 409 // angle1_2=0;
tanabe2000 3:8f4c81ad256a 410 // break;
tanabe2000 3:8f4c81ad256a 411 // case 36:
tanabe2000 3:8f4c81ad256a 412 // demoX = 0.0;
tanabe2000 3:8f4c81ad256a 413 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 414 // break;
tanabe2000 3:8f4c81ad256a 415 // case 37:
tanabe2000 3:8f4c81ad256a 416 // demoX = 0.5;
tanabe2000 3:8f4c81ad256a 417 // demoY = 0.0;
tanabe2000 3:8f4c81ad256a 418 // break;
tanabe2000 3:8f4c81ad256a 419 //
tanabe2000 3:8f4c81ad256a 420 // }
tanabe2000 3:8f4c81ad256a 421 //
tanabe2000 3:8f4c81ad256a 422 // if(!receiveSuccessed) omni.computeXY(0.0, 0.0, 0.0);
tanabe2000 3:8f4c81ad256a 423 // else {
tanabe2000 3:8f4c81ad256a 424 // if(con.getButton2(1) == 0) {
tanabe2000 3:8f4c81ad256a 425 // for(int i = 0; i < 4; i++) stick[i] = con.getStick(i);
tanabe2000 3:8f4c81ad256a 426 // if((-0.1 < stick[2] )&& (stick[2] < 0.1)) omni.computeXY(stick[0]/3,stick[1]/3, -1*anglePID.compute());
tanabe2000 3:8f4c81ad256a 427 // else {
tanabe2000 3:8f4c81ad256a 428 // omni.computeXY(stick[0]/3,stick[1]/3, -1*stick[2]/4.0);
tanabe2000 3:8f4c81ad256a 429 // ofsetNowAngle = nowAngle;
tanabe2000 3:8f4c81ad256a 430 // attachAngle = 0;
tanabe2000 3:8f4c81ad256a 431 // }
tanabe2000 3:8f4c81ad256a 432 // } else omni.computeXY(demoX,demoY, -1*anglePID.compute());
tanabe2000 3:8f4c81ad256a 433 // }
tanabe2000 3:8f4c81ad256a 434 // for (int i = 0; i < 4; i++) {
tanabe2000 3:8f4c81ad256a 435 // speed[i] = omni.wheel[i];
tanabe2000 3:8f4c81ad256a 436 // wheels[i]->setSpeed(speed[i]);
tanabe2000 3:8f4c81ad256a 437 //// debugpc.printf("%1d: %.3f, ", i, speed[i]);
tanabe2000 3:8f4c81ad256a 438 // }
tanabe2000 3:8f4c81ad256a 439 //
tanabe2000 3:8f4c81ad256a 440 //// }
tanabe2000 3:8f4c81ad256a 441 // dt = t.read(); // Calculate delta time
tanabe2000 3:8f4c81ad256a 442 //
tanabe2000 3:8f4c81ad256a 443 // if((con.getButton2(0) == 0) && (dt >= 0.5)) {
tanabe2000 3:8f4c81ad256a 444 // mode++;
tanabe2000 3:8f4c81ad256a 445 // t.reset();
tanabe2000 3:8f4c81ad256a 446 // }
tanabe2000 3:8f4c81ad256a 447 // if(mode > 37) mode = 0;
tanabe2000 3:8f4c81ad256a 448 //// debugpc.printf("mode = %d,time = %f\r\n",mode,dt);
tanabe2000 3:8f4c81ad256a 449 //
tanabe2000 3:8f4c81ad256a 450 //
tanabe2000 3:8f4c81ad256a 451 //}
tanabe2000 3:8f4c81ad256a 452 //
tanabe2000 3:8f4c81ad256a 453 //void GakuBot::autoMode2()
tanabe2000 3:8f4c81ad256a 454 //{
tanabe2000 3:8f4c81ad256a 455 // for(int i = 0; i < 3; i++) fire[i]->setSpeed(0);
tanabe2000 3:8f4c81ad256a 456 //
tanabe2000 3:8f4c81ad256a 457 //
tanabe2000 3:8f4c81ad256a 458 // omni.computeXY(demoX,demoY, -1*anglePID.compute());
tanabe2000 3:8f4c81ad256a 459 // for (int i = 0; i < 4; i++) {
tanabe2000 3:8f4c81ad256a 460 // speed[i] = omni.wheel[i];
tanabe2000 3:8f4c81ad256a 461 // wheels[i]->setSpeed(speed[i]);
tanabe2000 3:8f4c81ad256a 462 //// debugpc.printf("%1d: %.3f, ", i, speed[i]);
tanabe2000 3:8f4c81ad256a 463 // }
tanabe2000 3:8f4c81ad256a 464 //// }
tanabe2000 3:8f4c81ad256a 465 // dt = t.read();
tanabe2000 3:8f4c81ad256a 466 //
tanabe2000 3:8f4c81ad256a 467 // if((con.getButton2(0) == 0) && (dt >= 0.5)) {
tanabe2000 3:8f4c81ad256a 468 // mode++;
tanabe2000 3:8f4c81ad256a 469 // t.reset();
tanabe2000 3:8f4c81ad256a 470 // }
tanabe2000 3:8f4c81ad256a 471 // if(mode > 33) mode = 0;
tanabe2000 3:8f4c81ad256a 472 //// debugpc.printf("mode = %d,time = %f\r\n",mode,dt);
tanabe2000 3:8f4c81ad256a 473 //}