main
Dependents: 00_yotsuba 200_yotsuba_21 200_yotuba_21_uiChange
robot.cpp@6:dee6041c3d15, 2020-02-01 (annotated)
- Committer:
- piroro4560
- Date:
- Sat Feb 01 15:12:19 2020 +0000
- Revision:
- 6:dee6041c3d15
- Parent:
- 5:f5e79163d0eb
add count camera signal
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
piroro4560 | 0:3f87ec23c3cf | 1 | #include "robot.h" |
piroro4560 | 0:3f87ec23c3cf | 2 | |
piroro4560 | 4:a4f94f186ba0 | 3 | Robot::Robot() : |
piroro4560 | 4:a4f94f186ba0 | 4 | spin(P, I, D, interval), |
piroro4560 | 1:6a5065829cfc | 5 | pc(USBTX, USBRX, 115200), |
piroro4560 | 1:6a5065829cfc | 6 | shot(kicker), |
piroro4560 | 4:a4f94f186ba0 | 7 | drib(ESC), |
piroro4560 | 4:a4f94f186ba0 | 8 | |
piroro4560 | 4:a4f94f186ba0 | 9 | but(USER_BUTTON), |
piroro4560 | 4:a4f94f186ba0 | 10 | dip1(dip1in), |
piroro4560 | 4:a4f94f186ba0 | 11 | dip2(dip2in), |
piroro4560 | 4:a4f94f186ba0 | 12 | dip3(dip3in), |
piroro4560 | 4:a4f94f186ba0 | 13 | b1(b1in), |
piroro4560 | 4:a4f94f186ba0 | 14 | b2(b2in), |
piroro4560 | 4:a4f94f186ba0 | 15 | lcd(PB_9, PB_8) |
piroro4560 | 1:6a5065829cfc | 16 | |
piroro4560 | 0:3f87ec23c3cf | 17 | { |
piroro4560 | 4:a4f94f186ba0 | 18 | |
piroro4560 | 0:3f87ec23c3cf | 19 | omni.wheel[0].setRadian(PI/4 * 1); |
piroro4560 | 0:3f87ec23c3cf | 20 | omni.wheel[1].setRadian(PI/4 * 3); |
piroro4560 | 0:3f87ec23c3cf | 21 | omni.wheel[2].setRadian(PI/4 * 5); |
piroro4560 | 0:3f87ec23c3cf | 22 | omni.wheel[3].setRadian(PI/4 * 7); |
piroro4560 | 4:a4f94f186ba0 | 23 | motor[0] = new KohiMD(motor1); |
piroro4560 | 4:a4f94f186ba0 | 24 | motor[1] = new KohiMD(motor2); |
piroro4560 | 4:a4f94f186ba0 | 25 | motor[2] = new KohiMD(motor3); |
piroro4560 | 4:a4f94f186ba0 | 26 | motor[3] = new KohiMD(motor4); |
piroro4560 | 1:6a5065829cfc | 27 | spin.setInputLimits(-180, 180); |
THtakahiro702286 | 5:f5e79163d0eb | 28 | spin.setOutputLimits(-0.4,0.4); |
piroro4560 | 1:6a5065829cfc | 29 | spin.setBias(0); |
piroro4560 | 1:6a5065829cfc | 30 | spin.setMode(1); |
piroro4560 | 1:6a5065829cfc | 31 | spin.setSetPoint(0); |
piroro4560 | 4:a4f94f186ba0 | 32 | shot.setkickperiod(0.8); |
piroro4560 | 1:6a5065829cfc | 33 | shot.setoutputtime(0.01); |
piroro4560 | 4:a4f94f186ba0 | 34 | drib.setspeed(0.0); |
piroro4560 | 4:a4f94f186ba0 | 35 | but.mode(PullUp); |
THtakahiro702286 | 5:f5e79163d0eb | 36 | startb = 0; |
piroro4560 | 6:dee6041c3d15 | 37 | linecount[0] = 0; |
piroro4560 | 6:dee6041c3d15 | 38 | linecount[1] = 0; |
piroro4560 | 6:dee6041c3d15 | 39 | linecount[2] = 0; |
piroro4560 | 6:dee6041c3d15 | 40 | linecount[3] = 0; |
piroro4560 | 6:dee6041c3d15 | 41 | lineAngle = 0; |
piroro4560 | 0:3f87ec23c3cf | 42 | } |
piroro4560 | 0:3f87ec23c3cf | 43 | |
piroro4560 | 4:a4f94f186ba0 | 44 | void Robot::start() |
piroro4560 | 0:3f87ec23c3cf | 45 | { |
piroro4560 | 4:a4f94f186ba0 | 46 | |
piroro4560 | 4:a4f94f186ba0 | 47 | while (true) { |
piroro4560 | 6:dee6041c3d15 | 48 | pc.printf("%f\r\n", sensor.angle ); |
piroro4560 | 6:dee6041c3d15 | 49 | // sensor.blueAngle, sensor.blueRange/*, sensor.yellowAngle, sensor.yellowRange, sensor.ballAngle, sensor.ballRange */); |
piroro4560 | 6:dee6041c3d15 | 50 | if (!/*but*/2) startb=1; |
piroro4560 | 4:a4f94f186ba0 | 51 | spin.setProcessValue(sensor.angleLimit); |
piroro4560 | 4:a4f94f186ba0 | 52 | theta = sensor.ballAngle * ballExtra * PI / 180.0; |
piroro4560 | 4:a4f94f186ba0 | 53 | spin_power = spin.compute(); |
piroro4560 | 6:dee6041c3d15 | 54 | if (sensor.ballkeepcount > 100) { |
piroro4560 | 6:dee6041c3d15 | 55 | drib.setspeed(0.5); |
piroro4560 | 6:dee6041c3d15 | 56 | shot.outPut(); |
piroro4560 | 4:a4f94f186ba0 | 57 | if(dip1) |
piroro4560 | 4:a4f94f186ba0 | 58 | { |
piroro4560 | 6:dee6041c3d15 | 59 | // shot.outPut(); |
piroro4560 | 4:a4f94f186ba0 | 60 | if(!sensor.bluex) omni.computeCircular(1,0, spin_power); |
THtakahiro702286 | 5:f5e79163d0eb | 61 | else omni.computeCircular(sensor.blueRange,PI / 180.0 * sensor.blueAngle, spin_power); |
piroro4560 | 4:a4f94f186ba0 | 62 | } |
piroro4560 | 4:a4f94f186ba0 | 63 | else |
piroro4560 | 4:a4f94f186ba0 | 64 | { |
piroro4560 | 6:dee6041c3d15 | 65 | // shot.outPut(); |
piroro4560 | 4:a4f94f186ba0 | 66 | if(!sensor.yellowx) omni.computeCircular(1,0, spin_power); |
THtakahiro702286 | 5:f5e79163d0eb | 67 | else omni.computeCircular(sensor.yellowRange,PI / 180.0 * sensor.yellowAngle, spin_power); |
piroro4560 | 4:a4f94f186ba0 | 68 | } |
piroro4560 | 4:a4f94f186ba0 | 69 | } |
piroro4560 | 4:a4f94f186ba0 | 70 | else |
piroro4560 | 4:a4f94f186ba0 | 71 | { |
piroro4560 | 6:dee6041c3d15 | 72 | // for(int i=0; i<6; i++) pc.printf("%d ",sensor.line[i]); |
piroro4560 | 6:dee6041c3d15 | 73 | // pc.printf("\r\n"); |
THtakahiro702286 | 5:f5e79163d0eb | 74 | drib.setspeed(0.0); |
piroro4560 | 4:a4f94f186ba0 | 75 | omni.computeCircular(sensor.ballRange,PI / 180.0 * sensor.ballAngle + theta, spin_power); |
piroro4560 | 4:a4f94f186ba0 | 76 | } |
piroro4560 | 6:dee6041c3d15 | 77 | |
piroro4560 | 6:dee6041c3d15 | 78 | // pc.printf("%d ",(int)sensor.ballKeep); |
piroro4560 | 6:dee6041c3d15 | 79 | |
piroro4560 | 6:dee6041c3d15 | 80 | if (sensor.ballTimeout > 1.0 && sensor.ballTimeout < 2.0) |
piroro4560 | 6:dee6041c3d15 | 81 | { |
piroro4560 | 6:dee6041c3d15 | 82 | omni.computeCircular(1,PI,spin_power); |
piroro4560 | 6:dee6041c3d15 | 83 | } |
piroro4560 | 6:dee6041c3d15 | 84 | |
piroro4560 | 4:a4f94f186ba0 | 85 | if(sensor.line[0] || sensor.line[1]) |
piroro4560 | 4:a4f94f186ba0 | 86 | { |
piroro4560 | 6:dee6041c3d15 | 87 | linecount[0]+=2; |
piroro4560 | 4:a4f94f186ba0 | 88 | } |
piroro4560 | 6:dee6041c3d15 | 89 | else |
piroro4560 | 6:dee6041c3d15 | 90 | { |
piroro4560 | 6:dee6041c3d15 | 91 | linecount[0]--; |
piroro4560 | 6:dee6041c3d15 | 92 | } |
piroro4560 | 6:dee6041c3d15 | 93 | if(linecount[0] && !(linecount[1] + linecount[2] + linecount[3]) ) lineAngle = -1*PI/2.0; |
piroro4560 | 4:a4f94f186ba0 | 94 | if(sensor.line[2] || sensor.line[3]) |
piroro4560 | 4:a4f94f186ba0 | 95 | { |
piroro4560 | 6:dee6041c3d15 | 96 | linecount[1]+=2; |
piroro4560 | 4:a4f94f186ba0 | 97 | } |
piroro4560 | 6:dee6041c3d15 | 98 | else |
piroro4560 | 6:dee6041c3d15 | 99 | { |
piroro4560 | 6:dee6041c3d15 | 100 | linecount[1]--; |
piroro4560 | 6:dee6041c3d15 | 101 | } |
piroro4560 | 6:dee6041c3d15 | 102 | if(linecount[1] && !(linecount[0] + linecount[2] + linecount[3])) lineAngle = PI/2; |
piroro4560 | 4:a4f94f186ba0 | 103 | if(sensor.line[4]) |
piroro4560 | 4:a4f94f186ba0 | 104 | { |
piroro4560 | 6:dee6041c3d15 | 105 | linecount[2]+=2; |
piroro4560 | 4:a4f94f186ba0 | 106 | } |
piroro4560 | 6:dee6041c3d15 | 107 | else |
piroro4560 | 6:dee6041c3d15 | 108 | { |
piroro4560 | 6:dee6041c3d15 | 109 | linecount[2]--; |
piroro4560 | 6:dee6041c3d15 | 110 | } |
piroro4560 | 6:dee6041c3d15 | 111 | if(linecount[2] && !(linecount[1] + linecount[0] + linecount[3])) lineAngle = 0; |
piroro4560 | 4:a4f94f186ba0 | 112 | if(sensor.line[5]) |
piroro4560 | 4:a4f94f186ba0 | 113 | { |
piroro4560 | 6:dee6041c3d15 | 114 | linecount[3]+=2; |
piroro4560 | 6:dee6041c3d15 | 115 | } |
piroro4560 | 6:dee6041c3d15 | 116 | else |
piroro4560 | 6:dee6041c3d15 | 117 | { |
piroro4560 | 6:dee6041c3d15 | 118 | linecount[3]-=2; |
piroro4560 | 6:dee6041c3d15 | 119 | } |
piroro4560 | 6:dee6041c3d15 | 120 | if(linecount[3] && !(linecount[1] + linecount[2] + linecount[0])) lineAngle = PI; |
piroro4560 | 6:dee6041c3d15 | 121 | |
piroro4560 | 6:dee6041c3d15 | 122 | if(!(linecount[0] + linecount[1] + linecount[2] + linecount[3])) lineAngle = 0; |
piroro4560 | 6:dee6041c3d15 | 123 | |
piroro4560 | 6:dee6041c3d15 | 124 | for(int i = 0; i < 4; i++) |
piroro4560 | 6:dee6041c3d15 | 125 | { |
piroro4560 | 6:dee6041c3d15 | 126 | if(linecount[i] > 50) linecount[i] = 50; |
piroro4560 | 6:dee6041c3d15 | 127 | if(linecount[i] < 0) linecount[i] = 0; |
piroro4560 | 4:a4f94f186ba0 | 128 | } |
piroro4560 | 6:dee6041c3d15 | 129 | if(linecount[0] + linecount[1] + linecount[2] + linecount[3]) omni.computeCircular(1,lineAngle,spin_power); |
piroro4560 | 6:dee6041c3d15 | 130 | //if(sensor.line[0] || sensor.line[1]) |
piroro4560 | 6:dee6041c3d15 | 131 | // { |
piroro4560 | 6:dee6041c3d15 | 132 | // omni.computeCircular(1,-1*PI/2.0,spin_power); |
piroro4560 | 6:dee6041c3d15 | 133 | // |
piroro4560 | 6:dee6041c3d15 | 134 | // } |
piroro4560 | 6:dee6041c3d15 | 135 | // if(sensor.line[2] || sensor.line[3]) |
piroro4560 | 6:dee6041c3d15 | 136 | // { |
piroro4560 | 6:dee6041c3d15 | 137 | // omni.computeCircular(1,PI/2.0,spin_power); |
piroro4560 | 6:dee6041c3d15 | 138 | // } |
piroro4560 | 6:dee6041c3d15 | 139 | // if(sensor.line[4]) |
piroro4560 | 6:dee6041c3d15 | 140 | // { |
piroro4560 | 6:dee6041c3d15 | 141 | // omni.computeCircular(1,0,spin_power); |
piroro4560 | 6:dee6041c3d15 | 142 | // } |
piroro4560 | 6:dee6041c3d15 | 143 | // if(sensor.line[5]) |
piroro4560 | 6:dee6041c3d15 | 144 | // { |
piroro4560 | 6:dee6041c3d15 | 145 | // omni.computeCircular(1,PI,spin_power); |
piroro4560 | 6:dee6041c3d15 | 146 | // } |
piroro4560 | 4:a4f94f186ba0 | 147 | |
piroro4560 | 4:a4f94f186ba0 | 148 | if (startb) { |
piroro4560 | 6:dee6041c3d15 | 149 | omni0wheel = omni.wheel[0]; |
piroro4560 | 6:dee6041c3d15 | 150 | omni1wheel = omni.wheel[1]; |
piroro4560 | 4:a4f94f186ba0 | 151 | omni2wheel = omni.wheel[2]; |
THtakahiro702286 | 5:f5e79163d0eb | 152 | omni3wheel = omni.wheel[3]; |
piroro4560 | 6:dee6041c3d15 | 153 | motor[0]->setSpeed(omni0wheel); |
piroro4560 | 6:dee6041c3d15 | 154 | motor[1]->setSpeed(omni1wheel); |
piroro4560 | 6:dee6041c3d15 | 155 | motor[2]->setSpeed(omni2wheel);/////////////////////////////////// |
piroro4560 | 6:dee6041c3d15 | 156 | motor[3]->setSpeed(omni3wheel); |
THtakahiro702286 | 5:f5e79163d0eb | 157 | // motor[3]->setSpeed(-omni3wheel); |
piroro4560 | 4:a4f94f186ba0 | 158 | } else { |
piroro4560 | 4:a4f94f186ba0 | 159 | motor[0]->setSpeed(0); |
piroro4560 | 4:a4f94f186ba0 | 160 | motor[1]->setSpeed(0); |
piroro4560 | 4:a4f94f186ba0 | 161 | motor[2]->setSpeed(0); |
piroro4560 | 4:a4f94f186ba0 | 162 | motor[3]->setSpeed(0); |
piroro4560 | 4:a4f94f186ba0 | 163 | } |
piroro4560 | 4:a4f94f186ba0 | 164 | |
piroro4560 | 4:a4f94f186ba0 | 165 | |
piroro4560 | 4:a4f94f186ba0 | 166 | if (!startb) { |
piroro4560 | 4:a4f94f186ba0 | 167 | if (!b1) { |
piroro4560 | 4:a4f94f186ba0 | 168 | if (dip3) { |
piroro4560 | 4:a4f94f186ba0 | 169 | sensor.jy.jyroReset(); |
piroro4560 | 4:a4f94f186ba0 | 170 | } else { |
piroro4560 | 4:a4f94f186ba0 | 171 | shot.outPut(); |
piroro4560 | 4:a4f94f186ba0 | 172 | } |
piroro4560 | 4:a4f94f186ba0 | 173 | } |
piroro4560 | 4:a4f94f186ba0 | 174 | } |
piroro4560 | 4:a4f94f186ba0 | 175 | |
piroro4560 | 2:fc5545ddf69a | 176 | } |
piroro4560 | 0:3f87ec23c3cf | 177 | } |