Quadrifoglio / Robot

main

Dependents:   00_yotsuba 200_yotsuba_21 200_yotuba_21_uiChange

robot.cpp@5:f5e79163d0eb, 2020-02-01 (annotated)

Committer:
THtakahiro702286
Date:
Sat Feb 01 03:02:18 2020 +0000
Revision:
5:f5e79163d0eb
Parent:
4:a4f94f186ba0
Child:
6:dee6041c3d15
Child:
7:dcc0faa86da7
ugoia ayasii

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 0:3f87ec23c3cf 37 }
piroro4560 0:3f87ec23c3cf 38
piroro4560 4:a4f94f186ba0 39 void Robot::start()
piroro4560 0:3f87ec23c3cf 40 {
piroro4560 4:a4f94f186ba0 41
piroro4560 4:a4f94f186ba0 42 while (true) {
THtakahiro702286 5:f5e79163d0eb 43 if (!but/*2*/) startb=1;
piroro4560 4:a4f94f186ba0 44 spin.setProcessValue(sensor.angleLimit);
piroro4560 4:a4f94f186ba0 45 theta = sensor.ballAngle * ballExtra * PI / 180.0;
piroro4560 4:a4f94f186ba0 46 spin_power = spin.compute();
piroro4560 4:a4f94f186ba0 47 if (sensor.ballKeep) {
piroro4560 4:a4f94f186ba0 48 drib.setspeed(0.5);
piroro4560 4:a4f94f186ba0 49 if(dip1)
piroro4560 4:a4f94f186ba0 50 {
piroro4560 4:a4f94f186ba0 51 shot.outPut();
piroro4560 4:a4f94f186ba0 52 if(!sensor.bluex) omni.computeCircular(1,0, spin_power);
THtakahiro702286 5:f5e79163d0eb 53 else omni.computeCircular(sensor.blueRange,PI / 180.0 * sensor.blueAngle, spin_power);
piroro4560 4:a4f94f186ba0 54
piroro4560 4:a4f94f186ba0 55 }
piroro4560 4:a4f94f186ba0 56 else
piroro4560 4:a4f94f186ba0 57 {
piroro4560 4:a4f94f186ba0 58 shot.outPut();
piroro4560 4:a4f94f186ba0 59 if(!sensor.yellowx) omni.computeCircular(1,0, spin_power);
THtakahiro702286 5:f5e79163d0eb 60 else omni.computeCircular(sensor.yellowRange,PI / 180.0 * sensor.yellowAngle, spin_power);
piroro4560 4:a4f94f186ba0 61 }
piroro4560 4:a4f94f186ba0 62 }
piroro4560 4:a4f94f186ba0 63 else
piroro4560 4:a4f94f186ba0 64 {
piroro4560 4:a4f94f186ba0 65 for(int i=0; i<6; i++) pc.printf("%d ",sensor.line[i]);
piroro4560 4:a4f94f186ba0 66 pc.printf("\r\n");
THtakahiro702286 5:f5e79163d0eb 67 drib.setspeed(0.0);
piroro4560 4:a4f94f186ba0 68 omni.computeCircular(sensor.ballRange,PI / 180.0 * sensor.ballAngle + theta, spin_power);
piroro4560 4:a4f94f186ba0 69 }
piroro4560 4:a4f94f186ba0 70 pc.printf("%d ",(int)sensor.ballKeep);
piroro4560 4:a4f94f186ba0 71 if(sensor.line[0] || sensor.line[1])
piroro4560 4:a4f94f186ba0 72 {
piroro4560 4:a4f94f186ba0 73 omni.computeCircular(1,-1*PI/2.0,spin_power);
piroro4560 4:a4f94f186ba0 74
piroro4560 4:a4f94f186ba0 75 }
piroro4560 4:a4f94f186ba0 76 if(sensor.line[2] || sensor.line[3])
piroro4560 4:a4f94f186ba0 77 {
piroro4560 4:a4f94f186ba0 78 omni.computeCircular(1,PI/2.0,spin_power);
piroro4560 4:a4f94f186ba0 79 }
piroro4560 4:a4f94f186ba0 80 if(sensor.line[4])
piroro4560 4:a4f94f186ba0 81 {
piroro4560 4:a4f94f186ba0 82 omni.computeCircular(1,0,spin_power);
piroro4560 4:a4f94f186ba0 83 }
piroro4560 4:a4f94f186ba0 84 if(sensor.line[5])
piroro4560 4:a4f94f186ba0 85 {
piroro4560 4:a4f94f186ba0 86 omni.computeCircular(1,PI,spin_power);
piroro4560 4:a4f94f186ba0 87 }
piroro4560 4:a4f94f186ba0 88
piroro4560 4:a4f94f186ba0 89 if (startb) {
piroro4560 4:a4f94f186ba0 90 omni2wheel = omni.wheel[2];
THtakahiro702286 5:f5e79163d0eb 91 omni3wheel = omni.wheel[3];
piroro4560 4:a4f94f186ba0 92 motor[0]->setSpeed(omni.wheel[0]);
piroro4560 4:a4f94f186ba0 93 motor[1]->setSpeed(omni.wheel[1]);
piroro4560 4:a4f94f186ba0 94 motor[2]->setSpeed(-omni2wheel);
piroro4560 4:a4f94f186ba0 95 motor[3]->setSpeed(omni.wheel[3]);
THtakahiro702286 5:f5e79163d0eb 96 // motor[3]->setSpeed(-omni3wheel);
piroro4560 4:a4f94f186ba0 97 } else {
piroro4560 4:a4f94f186ba0 98 motor[0]->setSpeed(0);
piroro4560 4:a4f94f186ba0 99 motor[1]->setSpeed(0);
piroro4560 4:a4f94f186ba0 100 motor[2]->setSpeed(0);
piroro4560 4:a4f94f186ba0 101 motor[3]->setSpeed(0);
piroro4560 4:a4f94f186ba0 102 }
piroro4560 4:a4f94f186ba0 103
piroro4560 4:a4f94f186ba0 104
piroro4560 4:a4f94f186ba0 105 if (!startb) {
piroro4560 4:a4f94f186ba0 106 if (!b1) {
piroro4560 4:a4f94f186ba0 107 if (dip3) {
piroro4560 4:a4f94f186ba0 108 sensor.jy.jyroReset();
piroro4560 4:a4f94f186ba0 109 } else {
piroro4560 4:a4f94f186ba0 110 shot.outPut();
piroro4560 4:a4f94f186ba0 111 }
piroro4560 4:a4f94f186ba0 112 }
piroro4560 4:a4f94f186ba0 113 }
piroro4560 4:a4f94f186ba0 114
piroro4560 2:fc5545ddf69a 115 }
piroro4560 0:3f87ec23c3cf 116 }