Quadrifoglio / Robot

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?

UserRevisionLine numberNew 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 }