EmaMaker
/
SPQR-Team1-2020
1
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

reintroduces semi axis locking. Some bug with the lines make the robot stop when it gets on one. Tilt with vshaped mirror reintroduced

code_midgen
EmaMaker 2020-01-29 18:56:49 +01:00
parent 9a8830f275
commit df0b37554c
13 changed files with 212 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
include/data_source_camera.h → include/data_source_camera_conicmirror.h
View File

@ -7,10 +7,10 @@
//#define unkn 0b01101001
#include "data_source.h"
class DataSourceCamera : public DataSource{
class DataSourceCameraConic : public DataSource{
public:
DataSourceCamera(HardwareSerial* ser, int baud);
DataSourceCameraConic(HardwareSerial* ser, int baud);
void test() override;
void readSensor() override;
int getValueAtk(bool);

26
include/data_source_camera_vshapedmirror.h Normal file
View File

@ -0,0 +1,26 @@
#pragma once
#include "data_source.h"
class DataSourceCameraVShaped : public DataSource{
public:
DataSourceCameraVShaped(HardwareSerial* ser, int baud);
void postProcess() override;
void test() override;
int fixCamIMU(int);
void readSensor() override;
int getValueAtk(bool);
int getValueDef(bool);
int goalOrientation, pAtk, pDef, imuOff, portx, valX, valY, valB, oldGoalX, oldGoalY, oldGoalB;
int cameraReady;
char value;
int startpY = 0;
int startpB = 0;
int endpY = 0;
int endpB = 0;
int datavalid = 0;
String valStringY = "";
String valStringB = "";
bool negateB, negateY;
};

9
include/drivecontroller.h
View File

@ -4,10 +4,11 @@
#include "motor.h"
//PID Constants
#define KP 0.9
#define KP 2.1
#define KI 0
#define KD 0
#define KD 0.05
#define UNLOCK_THRESH 800
class DriveController{
@ -23,7 +24,7 @@ class DriveController{
int vxp, vyp, vxn, vyn;
bool canUnlock;
elapsedMillis unlockTime;
unsigned long unlockTime;
private:
Motor* m1;

2
include/goalie.h
View File

@ -2,7 +2,7 @@
#include "game.h"
#include "sensors.h"
#include "data_source_camera.h"
#include "data_source_camera_vshapedmirror.h"
#define GOALIE_ATKSPD_LAT 255
#define GOALIE_ATKSPD_BAK 350

1
include/linesys_2019.h
View File

@ -36,5 +36,4 @@ class LineSys2019 : public LineSystem{
elapsedMillis exitTimer;
int outDir, outVel;
byte linesens;
unsigned long unlockTime;
};

5
include/sensors.h
View File

@ -10,7 +10,8 @@
#include <Arduino.h>
#include "data_source_bno055.h"
#include "data_source_ball.h"
#include "data_source_camera.h"
#include "data_source_camera_conicmirror.h"
#include "data_source_camera_vshapedmirror.h"
#include "data_source_us.h"
#include "motor.h"
#include "ds_ctrl.h"
@ -30,7 +31,7 @@ s_extr LineSys2019* linesCtrl;
s_extr DataSourceBNO055* compass;
s_extr DataSourceBall* ball;
s_extr DataSourceCamera* camera;
s_extr DataSourceCameraVShaped* camera;
s_extr DriveController* drive;
s_extr DataSourceBT* bt;

17
src/data_source_camera.cpp → src/data_source_camera_conicmirror.cpp
View File

@ -1,9 +1,9 @@
#include "data_source_camera.h"
#include "data_source_camera_conicmirror.h"
#include "sensors.h"
DataSourceCamera::DataSourceCamera(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){}
DataSourceCameraConic::DataSourceCameraConic(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){}
void DataSourceCamera :: readSensor(){
void DataSourceCameraConic :: readSensor(){
while(ser->available() > 0){
value = (int)ser->read();
//Serial.println(value);
@ -31,8 +31,9 @@ void DataSourceCamera :: readSensor(){
yAngle = (yAngle + 360) % 360;
bAngle = (bAngle + 360) % 360;
yAngleFix = yAngle - compass->getValue()*0.9 ;
bAngleFix = bAngle - compass->getValue()*0.9 ;
//Fixes with IMU
yAngleFix = yAngle - compass->getValue()*0.85 ;
bAngleFix = bAngle - compass->getValue()*0.85 ;
yDist = sqrt( (50-true_yy)*(50-true_yy) + (50-true_xy)*(50-true_xy) );
bDist = sqrt( (50-true_yb)*(50-true_yb) + (50-true_xb)*(50-true_xb) );
@ -52,14 +53,14 @@ void DataSourceCamera :: readSensor(){
}
int DataSourceCamera::getValueAtk(bool b){
int DataSourceCameraConic::getValueAtk(bool b){
return 0;
}
int DataSourceCamera::getValueDef(bool b){
int DataSourceCameraConic::getValueDef(bool b){
return 0;
}
void DataSourceCamera::test(){
void DataSourceCameraConic::test(){
goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu
update();
DEBUG.print(bAngle);

142
src/data_source_camera_vshapedmirror.cpp Normal file
View File

@ -0,0 +1,142 @@
#include "data_source_camera_vshapedmirror.h"
#include "sensors.h"
DataSourceCameraVShaped::DataSourceCameraVShaped(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){}
void DataSourceCameraVShaped :: readSensor(){
portx = 999;
while(ser->available() > 0) {
value = ser->read();
// if the incoming character is a 'Y', set the start packet flag
if (value == 'Y') {
startpY = 1;
}
// if the incoming character is a 'Y', set the start packet flag
if (value == 'B') {
startpB = 1;
}
// if the incoming character is a '.', set the end packet flag
if (value == 'y') {
endpY = 1;
}
// if the incoming character is a '.', set the end packet flag
if (value == 'b') {
endpB = 1;
}
if ((startpY == 1) && (endpY == 0)) {
if (isDigit(value)) {
// convert the incoming byte to a char and add it to the string:
valStringY += value;
}else if(value == '-'){
negateY = true;
}
}
if ((startpB == 1) && (endpB == 0)) {
if (isDigit(value)) {
// convert the incoming byte to a char and add it to the string:
valStringB += value;
}else if(value == '-'){
negateB = true;
}
}
if ((startpY == 1) && (endpY == 1)) {
valY = valStringY.toInt(); // valid data
if(negateY) valY *= -1;
valStringY = "";
startpY = 0;
endpY = 0;
negateY = false;
datavalid ++;
}
if ((startpB == 1) && (endpB == 1)) {
valB = valStringB.toInt(); // valid data
if(negateB) valB *= -1;
valStringB = "";
startpB = 0;
endpB = 0;
negateB = false;
datavalid ++;
}
}
}
void DataSourceCameraVShaped :: postProcess(){
if (valY != -74)
oldGoalY = valY;
if (valB != -74)
oldGoalB = valB;
if (valY == -74)
valY = oldGoalY;
if (valB == -74)
valB = oldGoalB;
// entro qui solo se ho ricevuto i pacchetti completi sia del blu che del giallo
if (datavalid > 1 ) {
if(goalOrientation == 1){
//yellow goalpost
pAtk = valY;
pDef = valB * -1;
}else{
//blue goalpost
pAtk = valB;
pDef = valY * -1;
}
datavalid = 0;
cameraReady = 1; //attivo flag di ricezione pacchetto
}
}
int DataSourceCameraVShaped::getValueAtk(bool fixed){
//attacco gialla
if(goalOrientation == HIGH){
if(fixed) return fixCamIMU(valY);
return valY;
}
//attacco blu
if(goalOrientation == LOW){
if(fixed) return fixCamIMU(valB);
return valB;
}
}
int DataSourceCameraVShaped::getValueDef(bool fixed){
//difendo gialla
if(goalOrientation == HIGH){
if(fixed) return fixCamIMU(valY);
return valY;
}
//difendo blu
if(goalOrientation == LOW){
if(fixed) return fixCamIMU(valB);
return valB;
}
}
void DataSourceCameraVShaped::test(){
goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu
DEBUG.print(pAtk);
DEBUG.print(" | ");
DEBUG.print(fixCamIMU(pAtk));
DEBUG.print(" --- ");
DEBUG.print(pDef);
DEBUG.print(" | ");
DEBUG.println(fixCamIMU(pDef));
delay(100);
}
int DataSourceCameraVShaped::fixCamIMU(int d){
if(compass->getValue() > 0 && compass->getValue() < 180) imuOff = compass->getValue();
else if (compass->getValue() <= 360 && compass->getValue() >= 180) imuOff = compass->getValue() - 360;
imuOff = constrain(imuOff*0.8, -30, 30);
return d + imuOff;
}

17
src/drivecontroller.cpp
View File

@ -32,6 +32,13 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
errorePre = 0;
pidfactor = 0;
integral = 0;
canUnlock = true;
unlockTime = 0;
vxp = 0;
vxn = 0;
vyp = 0;
vyn = 0;
}
void DriveController::prepareDrive(int dir, int speed, int tilt){
@ -52,6 +59,16 @@ void DriveController::drive(int dir, int speed, int tilt){
vx = ((speed * cosins[dir]));
vy = ((-speed * sins[dir]));
if((((vy < 0 && vxn == 1) || (vy > 0 && vxp == 1) || (vx < 0 && vyp == 1) || (vx > 0 && vyn == 1)) && canUnlock) || (millis() > this->unlockTime+UNLOCK_THRESH)) {
vxn = 0;
vxp = 0;
vyp = 0;
vyn = 0;
}
if((vy > 0 && vxn == 1) || (vy < 0 && vxp == 1)) vy = 0;
if((vx > 0 && vyp == 1) || (vx < 0 && vyn == 1)) vx = 0;
speed1 = ((vx * sins[m1->angle] ) + (vy * cosins[m1->angle] ));
speed2 = ((vx * sins[m2->angle]) + (vy * cosins[m2->angle]));
speed3 = -(speed1);

60
src/goalie.cpp
View File

@ -34,51 +34,11 @@ void Goalie::goalie(int plusang) {
if(ball->dir < 0) ball->dir = ball->dir + 360;
else ball->dir = ball->dir;
ball->b = ball->dir;
drive->prepareDrive(ball->dir, 300, 0);
}
/* drive->speed = 300;
drive->dir = drive->dir; */
/* if(ball->angle >= 350 || ball->angle <= 10) {
if(ball->distance > 190) atk_direction = 0;
else atk_direction = ball->angle;
atk_speed = GOALIE_ATKSPD_FRT;
}
if(ball->angle >= 90 && ball->angle <= 270) {
this->ballBack();
atk_speed = GOALIE_ATKSPD_BAK;
storcimentoPorta();
if(ball->angle > 340 || ball->angle < 20) drive->prepareDrive(ball->dir, 350, cstorc);
else drive->prepareDrive(ball->dir, 350, 0);
}
if(ball->angle > 10 && ball->angle < 30) {
atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG1;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(ball->angle >= 30 && ball->angle < 45) {
atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG2;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(ball->angle >= 45 && ball->angle < 90) {
atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG3;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(ball->angle > 270 && ball->angle <= 315) {
atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG3_COR;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(ball->angle > 315 && ball->angle <= 330) {
atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG2_COR;
atk_speed = GOALIE_ATKSPD_LAT;
}
if(ball->angle > 330 && ball->angle < 350) {
atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG1_COR;
atk_speed = GOALIE_ATKSPD_LAT;
}
this->storcimentoPorta();
if((ball->angle >= 330 || ball->angle <= 30) && ball->distance > 190) { //storcimento
atk_speed = GOALIE_ATKSPD_STRK; //dove i gigahertz hanno fallito
drive->prepareDrive(atk_direction, atk_speed, cstorc);
}
else drive->prepareDrive(atk_direction, atk_speed); */
}
void Goalie::storcimentoPorta() {
@ -86,18 +46,4 @@ void Goalie::storcimentoPorta() {
else if (camera->getValueAtk(true) < -3) cstorc-=9;
else cstorc *= 0.7;
cstorc = constrain(cstorc, -45, 45);
}
void Goalie::ballBack() {
/* if(ball->distance > 130) ball->plusang = GOALIE_ATKDIR_PLUSANGBAK;
else ball->plusang = 0;
if(ball->angle > 180) ball->degrees2 = ball->angle - 360;
else ball->degrees2 = ball->angle;
if(ball->degrees2 > 0) ball->dir = ball->angle + ball->plusang; //45 con 8 ruote
else ball->dir = ball->angle - ball->plusang; //45 con 8 ruote
if(ball->dir < 0) ball->dir = ball->dir + 360;
else ball->dir = ball->dir;
atk_direction = ball->dir; */
}

4
src/linesys_2019.cpp
View File

@ -78,7 +78,7 @@ void LineSys2019::outOfBounds(){
if (exitTimer <= EXTIME){
//fase di rientro
if(linesens == 15) linesens = linesensOldY | linesensOldX; //ZOZZATA MAXIMA
unlockTime = millis();
drive->unlockTime = millis();
if(linesens == 1) outDir = 180;
else if(linesens == 2) outDir = 270;
@ -153,7 +153,7 @@ void LineSys2019::outOfBounds(){
if(linesensOldY == 4) drive->vyn = 1;
else if(linesensOldY == 1) drive->vyp = 1;
}
drive->canUnlock = true;
linesens = 0;
linesensOldY = 0;
linesensOldX = 0;

4
src/main.cpp
View File

@ -16,12 +16,10 @@ void setup() {
void loop() {
updateSensors();
//camera->test();
// camera->test();
goalie->play(role==1);
keeper->play(role==0);
camera->test();
// Last thing to do: movement
drive->drivePrepared();
}

2
src/sensors.cpp
View File

@ -15,7 +15,7 @@ void initSensors(){
drive = new DriveController(new Motor(12, 11, 2, 45),new Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315));
compass = new DataSourceBNO055();
ball = new DataSourceBall(&Serial4, 57600);
camera = new DataSourceCamera(&Serial2, 19200);
camera = new DataSourceCameraVShaped(&Serial2, 19200);
usCtrl = new DataSourceCtrl(dUs);
bt = new DataSourceBT(&Serial3, 115200);
}