EmaMaker
/
SPQR-Team-2019-REVAMPED
1
1
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

we now have a roller

pull/1/head
EmaMaker 2021-05-07 21:39:03 +02:00
parent e71c49efd1
commit 9321e6ff0b
21 changed files with 340 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/motors_movement/drivecontroller.h
View File

@ -38,6 +38,7 @@ class DriveController{
float updatePid();
float torad(float f);
void resetDrive();
void stopAll();
int vxp, vyp, vxn, vyn;
bool canUnlock;

1
include/motors_movement/motor.h
View File

@ -12,6 +12,7 @@ class Motor {
Motor();
void drive(int speed);
void test();
void stop();
public:
int angle;

20
include/motors_movement/roller.h Normal file
View File

@ -0,0 +1,20 @@
#pragma once
#include "ESC.h"
#define ROLLER_DEFAULT_SPEED 1300
class Roller{
public:
Roller(int pinPwm_, int pinSense, int MIN_, int MAX_, int ARM_);
void setup();
void speed(int);
void update();
public:
ESC* roller;
int pinPwm, pinSense, MIN, MAX, ARM;
bool roller_arm_setup, roller_armed;
int roller_setup_phase, roller_counter;
};

8
include/sensors/data_source_ball.h
View File

@ -1,5 +1,11 @@
#pragma once
#include "behaviour_control/data_source.h"
#include <Arduino.h>
#define MOUTH_MIN_ANGLE 340
#define MOUTH_MAX_ANGLE 20
#define MOUTH_DISTANCE 110
#define MOUTH_MAX_DISTANCE 140
class DataSourceBall : public DataSource{
@ -7,6 +13,8 @@ class DataSourceBall : public DataSource{
DataSourceBall(HardwareSerial* ser, int baud);
void postProcess() override;
void test() override;
bool isInMouth();
bool isInMouthMaxDistance();
int angle, distance, angleFix;
bool ballSeen;

2
include/sensors/data_source_camera_conicmirror.h
View File

@ -35,7 +35,7 @@ class DataSourceCameraConic : public DataSource{
true_xy_fixed, true_yb_fixed, true_yy_fixed;
bool data_received = false, start = false, end = false;
int goalOrientation, pAtk, pDef;
int goalOrientation, old_goalOrientation, pAtk, pDef;
int value;
ComplementaryFilter *filter_yy, *filter_xy, *filter_yb, *filter_xb, *filter_yy_fix, *filter_xy_fix, *filter_yb_fix, *filter_xb_fix;

9
include/sensors/sensors.h
View File

@ -10,11 +10,12 @@
#include <Arduino.h>
#include "behaviour_control/ds_ctrl.h"
#include "motors_movement/drivecontroller.h"
#include "motors_movement/motor.h"
#include "motors_movement/roller.h"
#include "motors_movement/drivecontroller.h"
#include "systems/systems.h"
#include "systems/lines/linesys_camera.h"
#include "systems/position/positionsys_zone.h"
#include "systems/systems.h"
#include "sensors/data_source_ball.h"
#include "sensors/data_source_bt.h"
#include "sensors/data_source_bno055.h"
@ -36,4 +37,6 @@ s_extr DriveController* drive;
s_extr DataSourceBT* bt;
s_extr int role;
s_extr int robot_indentifier;
s_extr int robot_indentifier;
s_extr Roller* roller;

4
include/strategy_roles/striker.h
View File

@ -25,8 +25,4 @@ class Striker : public Game{
float cstorc;
bool gotta_tilt;
ComplementaryFilter* filter;
};

4
include/systems/position/positionsys_camera.h
View File

@ -7,8 +7,8 @@
/*Camera translation: because of mechanical imprecision, the center of the camera and the center of the cone mirror may not coincide
To overcome this, each coordinate needs to be shifted by some amount, defined on a per-robot basis that needs to be recalibrated each time.
These values need to be subtracted from the coords used in setMoveSetpoints*/
#define CAMERA_TRANSLATION_X 10
#define CAMERA_TRANSLATION_Y 0
#define CAMERA_TRANSLATION_X 4
#define CAMERA_TRANSLATION_Y 4
//Camera center: those setpoints correspond to what we consider the center of the field
#define CAMERA_CENTER_X 0
#define CAMERA_CENTER_Y 0

3
include/vars.h
View File

@ -18,4 +18,7 @@ in the 32U4 code*/
#define SWITCH_DX 38
#define SWITCH_ID 33
#define ROLLER_INA 34
#define ROLLER_INB 35
extr float sins[360], cosins[360];

86
lib/RC_ESC/ESC.cpp Normal file
View File

@ -0,0 +1,86 @@
/*
* Electronic Speed Controller (ESC) - Library
*
*
*/
#include "ESC.h"
// < Constructor >
/*
* Sets the proper pin to output.
*/
ESC::ESC(byte ESC_pin, int outputMin, int outputMax, int armVal)
{
oPin = ESC_pin;
oMin = outputMin;
oMax = outputMax;
oArm = armVal;
}
// < Destructor >
ESC::~ESC()
{
// Nothing to destruct
}
/*
* Calibrate the maximum and minimum PWM signal the ESC is expecting
* depends on the outputMin, outputMax values from the constructor
*/
void ESC::calib(void)
{
myESC.attach(oPin); // attaches the ESC on pin oPin to the ESC object
myESC.writeMicroseconds(oMax);
delay(calibrationDelay);
myESC.writeMicroseconds(oMin);
delay(calibrationDelay);
arm();
}
/*
* Sent a signal to Arm the ESC
* depends on the Arming value from the constructor
*/
void ESC::arm(void)
{
myESC.attach(oPin); // attaches the ESC on pin oPin to the ESC object
myESC.writeMicroseconds (oArm);
}
/*
* Sent a signal to stop the ESC
* depends on the ESC stop pulse value
*/
void ESC::stop(void)
{
myESC.writeMicroseconds (stopPulse);
}
/*
* Sent a signal to set the ESC speed
* depends on the calibration minimum and maximum values
*/
void ESC::speed(int outputESC)
{
oESC = constrain(outputESC, oMin, oMax);
myESC.writeMicroseconds(oESC);
}
/*
* Set the current calibration delay in miliseconds
*
*/
void ESC::setCalibrationDelay(uint32_t calibration_delay)
{
calibrationDelay = calibration_delay;
}
/*
* Set the current Stop pulse in microseconds
*
*/
void ESC::setStopPulse(uint32_t stop_pulse)
{
stopPulse = stop_pulse;
}

47
lib/RC_ESC/ESC.h Normal file
View File

@ -0,0 +1,47 @@
/*
* Electronic Speed Controller (ESC) - Library
*
*
*/
#ifndef ESC_Library
#define ESC_Library
#if (ARDUINO >= 100)
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#include <Servo.h> // Including the Servo library
class ESC
{
public:
ESC(byte ESC_pin, int outputMin = 1000, int outputMax = 2000, int armVal = 500);
~ESC();
void calib(void);
void arm(void);
void stop(void);
void speed(int ESC_val);
void setCalibrationDelay(uint32_t calibration_delay);
void setStopPulse(uint32_t stop_pulse);
private:
// < Local attributes >
// Hardware
byte oPin; // ESC output Pin
// Calibration
int oMin = 1000;
int oMax = 2000;
int oESC = 1000;
int oArm = 500;
uint32_t calibrationDelay = 8000; // Calibration delay (milisecond)
uint32_t stopPulse = 500; // Stop pulse (microseconds)
Servo myESC; // create servo object to control an ESC
};
#endif
/* Electronic Speed Controller (ESC) - Library */

7
src/main.cpp
View File

@ -7,6 +7,9 @@
#include "strategy_roles/games.h"
#include "vars.h"
#include "test_menu.h"
#include "motors_movement/roller.h"
void updateRoller();
TestMenu* testmenu;
@ -26,7 +29,7 @@ void setup() {
initStatusVector();
initSensors();
initGames();
delay(500);
drive->prepareDrive(0,0,0);
@ -35,7 +38,6 @@ void setup() {
tone(BUZZER, 220.00, 250);
}
void loop() {
updateSensors();
@ -46,7 +48,6 @@ void loop() {
striker->play(striker_condition);
keeper->play(keeper_condition);
testmenu->testMenu();
// Last thing to do: movement and update status vector

7
src/motors_movement/drivecontroller.cpp
View File

@ -157,4 +157,11 @@ void DriveController::resetDrive(){
CURRENT_DATA_WRITE.addvx = 0;
CURRENT_DATA_WRITE.addvy = 0;
prepareDrive(0,0,0);
}
void DriveController::stopAll(){
m1->stop();
m2->stop();
m3->stop();
m4->stop();
}

4
src/motors_movement/motor.cpp
View File

@ -35,6 +35,10 @@ void Motor::drive(int speed){
analogWrite(pinPwm, speed);
}
void Motor::stop(){
this->drive(0);
}
void Motor::test(){
digitalWriteFast(pinA, 0);
digitalWriteFast(pinB, 1);

95
src/motors_movement/roller.cpp Normal file
View File

@ -0,0 +1,95 @@
#include "motors_movement/roller.h"
#include "vars.h"
/* Our PCB is 3.3V, the ESC uses 5V and has an onboard 5V/3A regulated output
To command the ESC we use a Sparkfun's 3.3V-5V board, which also does 5V-3.3V.
To PWM command the ESC we pass a PWM pin to the adapter board.
We use the same adapter board to adapt the ESC's internally regulated 5V to 3.3V.
Since the ESC is turned on (and outputs) only when the motors are turned on, we can detect when the ESC is turned on and needs the startup sequence*/
unsigned long t = 0;
unsigned long t1 = 0;
Roller::Roller(int pinPwm_, int pinSense_, int MIN_, int MAX_, int ARM_){
pinPwm = pinPwm_;
pinSense = pinSense_;
MIN = MIN_;
MAX = MAX_;
ARM = ARM_;
roller_armed = false;
roller_arm_setup = false;
roller_setup_phase = 99;
roller_counter = 0;
pinMode(pinSense, INPUT_PULLUP);
roller = new ESC(pinPwm, MIN, MAX, ARM);
}
void Roller::setup(){
if(roller_setup_phase == 0){
roller->arm();
roller_setup_phase=1;
roller_counter = 0;
roller_armed = false;
}else if(roller_setup_phase == 1){
if(millis() - t1 > 1){
t1 = millis();
roller->speed(roller_counter);
roller_counter++;
if(roller_counter > 255) roller_setup_phase = 2;
}
}else if(roller_setup_phase == 2){
if(millis() - t1 > 1){
t1 = millis();
roller->speed(roller_counter);
roller_counter--;
if(roller_counter <= 0) roller_setup_phase = 3;
}
}else if(roller_setup_phase == 3){
roller_setup_phase = 4;
roller_armed = true;
}
// DEBUG.print(roller_setup_phase);
// DEBUG.print(" --- ");
// DEBUG.println(roller_counter);
// roller->arm();
// delay(1000);
// for(int i = MIN; i < MAX; i++){
// roller->speed(i);
// delay(1);
// }
// for(int i = MAX; i > MIN; i--){
// roller->speed(i);
// delay(1);
// }
}
void Roller::update(){
bool arm = digitalReadFast(pinSense);
if(arm){
if(!roller_arm_setup){
roller_setup_phase = 0;
roller_arm_setup = true;
roller_armed = false;
}
}else{
roller_armed = false;
roller_arm_setup = false;
}
this->setup();
if(millis() - t > 10 && roller_armed){
roller->speed(1300);
t = millis();
}
}
void Roller::speed(int speed){
roller->speed(speed);
}

8
src/sensors/data_source_ball.cpp
View File

@ -38,4 +38,12 @@ void DataSourceBall :: test(){
// }else{
// DEBUG.println("Not seeing ball");
// }
}
bool DataSourceBall::isInMouth(){
return (CURRENT_DATA_READ.ballAngle > MOUTH_MIN_ANGLE || CURRENT_DATA_READ.ballAngle < MOUTH_MAX_ANGLE ) && CURRENT_DATA_READ.ballDistance<=MOUTH_DISTANCE;
}
bool DataSourceBall::isInMouthMaxDistance(){
return (CURRENT_DATA_READ.ballAngle>MOUTH_MIN_ANGLE || CURRENT_DATA_READ.ballAngle<MOUTH_MAX_ANGLE) && CURRENT_DATA_READ.ballDistance <= MOUTH_MAX_DISTANCE;
}

9
src/sensors/data_source_camera_conicmirror.cpp
View File

@ -31,6 +31,9 @@ DataSourceCameraConic::DataSourceCameraConic(HardwareSerial *ser_, int baud) : D
true_xy_fixed = 0;
true_yy_fixed = 0;
goalOrientation = 0;
old_goalOrientation = 0;
filter_yy = new ComplementaryFilter(FILTER_YY_COEFF);
filter_xy = new ComplementaryFilter(FILTER_YX_COEFF);
filter_yb = new ComplementaryFilter(FILTER_BY_COEFF);
@ -120,9 +123,6 @@ void DataSourceCameraConic ::computeCoordsAngles() {
true_xy_fixed = (true_xy*(cos(angleFix))) - (true_yy*(sin(angleFix)));
true_yy_fixed = (true_xy*(sin(angleFix))) + (true_yy*(cos(angleFix)));
yAngleFix = 90 - (atan2(true_yy_fixed, true_xy_fixed) * 180 / 3.14);
bAngleFix = 90 - (atan2(true_yb_fixed, true_xb_fixed) * 180 / 3.14);
#ifdef CAMERA_CONIC_FILTER_POINTS
true_xb_fixed = filter_xb_fix->calculate(true_xb_fixed);
true_yb_fixed = filter_yb_fix->calculate(true_yb_fixed);
@ -130,6 +130,9 @@ void DataSourceCameraConic ::computeCoordsAngles() {
true_yy_fixed = filter_yy_fix->calculate(true_yy_fixed);
#endif
yAngleFix = 90 - (atan2(true_yy_fixed, true_xy_fixed) * 180 / 3.14);
bAngleFix = 90 - (atan2(true_yb_fixed, true_xb_fixed) * 180 / 3.14);
//Important: update status vector
CURRENT_INPUT_WRITE.cameraByte = value;
CURRENT_DATA_WRITE.cam_xb = true_xb;

8
src/sensors/sensors.cpp
View File

@ -7,22 +7,22 @@ void initSensors(){
pinMode(SWITCH_ID, INPUT);
drive = new DriveController(new Motor(12, 11, 4, 55), new Motor(25, 24, 5, 135), new Motor(27, 26, 2, 225), new Motor(29, 28, 3, 305));
//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(&Serial2, 57600);
//ball = new DataSourceBall(&Serial4, 57600);
camera = new DataSourceCameraConic(&Serial3, 19200);
//camera = new DataSourceCameraConic(&Serial2, 19200);
bt = new DataSourceBT(&Serial1, 115200);
//bt = new DataSourceBT(&Serial3, 115200);
roller = new Roller(30, 31, 1000, 2000, 500);
}
void updateSensors(){
role = digitalRead(SWITCH_DX);
camera->old_goalOrientation = camera ->goalOrientation;
camera->goalOrientation = digitalRead(SWITCH_SX);
robot_indentifier = digitalRead(SWITCH_ID);
compass->update();
ball->update();
camera->update();
roller->update();
}

40
src/strategy_roles/striker.cpp
View File

@ -1,11 +1,12 @@
#include "behaviour_control/status_vector.h"
#include "systems/position/positionsys_camera.h"
#include "sensors/sensors.h"
#include "sensors/data_source_ball.h"
#include "strategy_roles/striker.h"
#include "vars.h"
#include "math.h"
Striker::Striker() : Game()
{
init();
@ -24,8 +25,6 @@ void Striker::init()
cstorc = 0;
gotta_tilt = false;
filter = new ComplementaryFilter(0.7);
}
void Striker::realPlay()
@ -66,18 +65,29 @@ void Striker::striker()
drive->prepareDrive(dir, MAX_VEL_HALF, tilt());
}
int Striker::tilt()
{
if (CURRENT_DATA_READ.ballDistance <= STRIKER_ATTACK_DISTANCE) gotta_tilt = true;
if (CURRENT_DATA_READ.ballDistance > STRIKER_ATTACK_DISTANCE && gotta_tilt) gotta_tilt = false;
if (CURRENT_DATA_READ.atkSeen && gotta_tilt) return 0;
if (CURRENT_DATA_READ.ballAngle >= 345 || CURRENT_DATA_READ.ballAngle <= 15) atk_tilt = CURRENT_DATA_READ.angleAtkFix;
int Striker::tilt() {
if (ball->isInMouth() || (ball->isInMouthMaxDistance() && gotta_tilt)) gotta_tilt = true;
else gotta_tilt = false;
// if (CURRENT_DATA_READ.ballAngle >= 350 || CURRENT_DATA_READ.ballAngle <= 10)
// atk_tilt = (constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45) + 360) % 360;
// else if((CURRENT_DATA_READ.ballAngle > 345 && CURRENT_DATA_READ.ballAngle < 350) || (CURRENT_DATA_READ.ballAngle > 10 && CURRENT_DATA_READ.ballAngle < 15))
// atk_tilt = 0;
atk_tilt = filter->calculate(atk_tilt);
if(!gotta_tilt || !CURRENT_DATA_READ.atkSeen) {
roller->speed(roller->MIN);
atk_tilt *= 0.8;
if(atk_tilt <= 10) atk_tilt = 0;
}else{
roller->speed(ROLLER_DEFAULT_SPEED);
atk_tilt = constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45);
}
return atk_tilt;
// if (ball->isInMouth() || (ball->isInMouthMaxDistance() && gotta_tilt)) gotta_tilt = true;
// else gotta_tilt = false;
// if(!gotta_tilt || !CURRENT_DATA_READ.atkSeen) {
// roller->speed(roller->MIN);
// return 0;
// }else{
// roller->speed(ROLLER_DEFAULT_SPEED);
// return constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45);
// }
}

6
src/test_menu.cpp
View File

@ -55,6 +55,7 @@ void TestMenu::testMenu()
break;
case '3':
DEBUG.println("Motors Test. Lift up from ground and turn on S_MOT");
drive->stopAll();
drive->m1->test();
drive->m2->test();
drive->m3->test();
@ -126,6 +127,10 @@ void TestMenu::testMenu()
Serial2.write(0);
delay(1500);
break;
case 'r':
drive->stopAll();
flagtest = false;
break;
case 'h':
default:
DEBUG.println("Unknown command, here's a lil help for you :)");
@ -143,6 +148,7 @@ void TestMenu::testMenu()
DEBUG.println("9)Switches test");
DEBUG.println("u)Read Serial messages from 32u4");
DEBUG.println("s)Send test to 32u4 status LEDs");
DEBUG.println("r)Test roller");
flagtest = false;
}
}

12
utility/OpenMV/conic_eff_h7.py
View File

@ -45,11 +45,11 @@ blue_led.on()
##############################################################################
thresholds = [ (54, 73, -4, 17, 18, 73), # thresholds yellow goal
(36, 45, -7, 7, -35, -10)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
thresholds = [ (48, 83, -9, 18, 38, 82), # thresholds yellow goal
(45, 61, -11, 14, -42, -22)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (80, 0, 240, 230)
roi = (20, 0, 300, 220)
# Camera Setup ###############################################################
'''sensor.reset()xxxx
@ -68,9 +68,9 @@ sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_windowing(roi)
sensor.set_contrast(0)
sensor.set_saturation(0)
sensor.set_brightness(1)
sensor.set_contrast(3)
sensor.set_saturation(3)
sensor.set_brightness(3)
sensor.set_auto_whitebal(True)
sensor.set_auto_exposure(False, 6576)
sensor.set_auto_gain(True)