前のやつです。
Dependencies: FEP OmniPosition PID QEI R1307 TFmini ikarashiMDC linesSnsor omni_wheel
gakuBot/gakubot.cpp@5:c7643ae5835f, 2018-09-11 (annotated)
- Committer:
- tanabe2000
- Date:
- Tue Sep 11 15:35:59 2018 +0000
- Revision:
- 5:c7643ae5835f
- Parent:
- 3:8f4c81ad256a
changing coments
Who changed what in which revision?
User | Revision | Line number | New 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 | 5:c7643ae5835f | 70 | pid.setSetPoint(distance); |
tanabe2000 | 5:c7643ae5835f | 71 | pid.setBias(0.0); |
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 | 5:c7643ae5835f | 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 | 5:c7643ae5835f | 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 | 5:c7643ae5835f | 218 | // switch(mode) { |
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 | 5:c7643ae5835f | 228 | // |
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 | 5:c7643ae5835f | 276 | // |
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 | 5:c7643ae5835f | 324 | // |
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 | 5:c7643ae5835f | 372 | // |
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 | //} |