ank/RBE2002
1
Fork 0
Code Issues Activity

diff

Browse source
This commit is contained in:
Andy Killorin 2024-11-20 19:53:01 -05:00
parent 4e555f195f
commit 95197007d0
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
9 changed files with 95 additions and 74 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

9
lib/Chassis/src/chassis.cpp
View file

@ -155,4 +155,11 @@ void Chassis::SetTwist(float fwdSpeed, float rotSpeed)
float rightSpeed = fwdSpeed + rotSpeed * ROBOT_RADIUS;
SetWheelSpeeds(leftSpeed, rightSpeed);
}
}
// cm/s
float Chassis::GetAverageSpeed() {
float average_ticks_per_loop = (leftMotor.speed + rightMotor.speed)/2;
float average_cm_per_s = average_ticks_per_loop / LEFT_TICKS_PER_CM / CONTROL_LOOP_PERIOD_S;
return average_cm_per_s;
}

1
lib/Chassis/src/chassis.h
View file

@ -39,6 +39,7 @@ public:
void SetTwist(float fwdSpeed, float rotSpeed);
void SetWheelSpeeds(float, float);
void UpdateMotors(void);
float GetAverageSpeed();
protected:
/**

2
lib/LSM6/src/LSM6.cpp
View file

@ -358,4 +358,4 @@ int16_t LSM6::testReg(uint8_t address, regAddr reg)
{
return TEST_REG_ERROR;
}
}
}

10
lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
View file

@ -133,6 +133,8 @@ protected:
currentSetpoint += deltaSetpoint;
float error = currentSetpoint - speed;
//Serial.println(speed);
//Serial.println(targetSpeed);
sumError += error;
@ -165,13 +167,13 @@ protected:
if(debug)
{
Serial.print("target:");
Serial.print("");
Serial.print(targetSpeed);
Serial.print("\tspeed:");
Serial.print("\t");
Serial.print(speed);
Serial.print("\terror:");
Serial.print("\t");
Serial.print(error);
Serial.print("\teffort:");
Serial.print("\t");
Serial.print(effort / 10.0); // N.B. that we divide by 10 to make the graph cleaner
Serial.print('\n');
}

5
platformio.ini
View file

@ -22,8 +22,11 @@ lib_deps =
https://github.com/WPIRoboticsEngineering/openmv-apriltags
; https://github.com/gcl8a/LSM6
https://github.com/WPIRoboticsEngineering/Romi-32u4-utilities
https://github.com/gcl8a/Load-Cell-Amps
build_flags =
; -D__MOTOR_DEBUG__
; -D__LOOP_DEBUG__
; -D__IMU_DEBUG__
; -D__IMU_DEBUG__
; -D__TRACK_DEBUG__
; -DDARK

2
src/robot-cam.cpp
View file

@ -32,7 +32,7 @@ uint8_t Robot::FindAprilTags(void) {
if(camera.checkUART(tag))
{
if (tag.id < 10) { // activate blinkenlights
tagId = tag.id;
blinkTagId = tag.id;
}
Serial.print(F("Tag [cx="));
Serial.print(tag.cx);

6
src/robot-remote.cpp
View file

@ -17,7 +17,7 @@ IRDecoder decoder(IR_PIN);
void Robot::HandleKeyCode(int16_t keyCode)
{
Serial.println(keyCode);
//Serial.println(keyCode);
// Regardless of current state, if ENTER is pressed, go to idle state
if(keyCode == STOP_MODE) EnterIdleState();
@ -132,6 +132,7 @@ void Robot::HandleKeyCode(int16_t keyCode)
case VOLminus:
k = keyString.toInt() / 100.0;
Serial.print("Kp = "); Serial.println(k);
//lining_kP = k;
chassis.SetMotorKp(k);
keyString = "";
break;
@ -144,6 +145,7 @@ void Robot::HandleKeyCode(int16_t keyCode)
case VOLplus:
k = keyString.toInt() / 100.0;
Serial.print("Kd = "); Serial.println(k);
//lining_kD = k;
chassis.SetMotorKd(k);
keyString = "";
break;
@ -202,4 +204,4 @@ void Robot::EnterSetupMode(void)
{
Serial.println("-> SETUP");
robotCtrlMode = CTRL_SETUP;
}
}

114
src/robot.cpp
View file

@ -24,7 +24,9 @@ void Robot::InitializeRobot(void)
// The line sensor elements default to INPUTs, but we'll initialize anyways, for completeness
lineSensor.Initialize();
servo.attach();
servo.attach();
loadCellHX1.Init();
}
void Robot::EnterIdleState(void)
@ -83,6 +85,8 @@ void Robot::HandleTurnComplete(void)
void Robot::HandleOrientationUpdate(void)
{
prevEulerAngles = eulerAngles;
if(robotState == ROBOT_IDLE)
{
// TODO: You'll need to add code to LSM6 to update the bias
@ -115,16 +119,42 @@ void Robot::HandleOrientationUpdate(void)
float gyrodelt_y = (float)imu.g.y / (float)INT_MAX;
gyrodelt_y *= 245.0 / 104.0;
eulerAngles.y = (1-kappa) * (eulerAngles.y + gyrodelt_y) + kappa * acc_y;
#ifdef __IMU_DEBUG__
Serial.print(imu.gyroBias.z);
Serial.print('\t');
Serial.print(eulerAngles.z);
Serial.print('\t');
//Serial.print(millis());
//Serial.print('\t');
//Serial.print(imu.gyroBias.z);
//Serial.print('\t');
//Serial.print(eulerAngles.z);
//Serial.print('\t');
//Serial.print(imu.g.x);
//Serial.print('\t');
//Serial.print(imu.g.y);
//Serial.print('\t');
Serial.println(imu.g.z);
//Serial.println(imu.g.z);
//float g_lsb = 0.061 / 1000.;
//Serial.print(imu.a.x * g_lsb);
//Serial.print('\t');
//Serial.print(asin(imu.a.x * g_lsb) * 180 / PI);
//Serial.print('\t');
//Serial.println(eulerAngles.x);
//Serial.print('\t');
//Serial.print(imu.a.y * g_lsb);
//Serial.print('\t');
//Serial.println(imu.a.z * g_lsb);
Serial.print(eulerAngles.x + gyrodelt_x);
Serial.print('\t');
Serial.print(acc_x);
Serial.print('\t');
Serial.println(eulerAngles.x);
//Serial.print(eulerAngles.x);
//Serial.print('\t');
//Serial.print(eulerAngles.y);
//Serial.print('\t');
//Serial.println(eulerAngles.z);
#endif
}
@ -155,8 +185,8 @@ void Robot::LineFollowingUpdate(void)
float setpoint = 3.5;
#endif
float lineError = setpoint - lineSensor.CalcError();
float lineError = setpoint - lineSensor.CalcError();
float powErr = (lineError*lineError);
powErr = (lineError < 0 ? -powErr : powErr);
@ -246,55 +276,14 @@ void Robot::LineFollowingUpdate(void)
*/
void Robot::HandleIntersection(void)
{
Serial.print("X: ");
if(robotState == ROBOT_LINING)
{
switch(nodeTo)
{
case NODE_START:
if(nodeFrom == NODE_1)
EnterIdleState();
break;
case NODE_1:
// By default, we'll continue on straight
if(nodeFrom == NODE_START)
{
nodeTo = NODE_2;
}
else if(nodeFrom == NODE_2)
{
nodeTo = NODE_START;
}
nodeFrom = NODE_1;
break;
case NODE_2:
// By default, we'll continue on straight
if(nodeFrom == NODE_1)
{
nodeTo = NODE_3;
}
else if(nodeFrom == NODE_3)
{
nodeTo = NODE_1;
}
nodeFrom = NODE_2;
break;
case NODE_3:
// By default, we'll bang a u-ey
if(nodeFrom == NODE_2)
{
nodeTo = NODE_2;
nodeFrom = NODE_3;
EnterTurn(180);
}
break;
default:
break;
}
Serial.print(nodeFrom);
Serial.print("->");
Serial.print(nodeTo);
Serial.print('\n');
//Serial.print("Y: ");
//Serial.println(pj);
if(robotState == ROBOT_LINING) {
// update location
if (currDirection == EAST) pi++;
if (currDirection == WEST) pi--;
if (currDirection == NORTH) pj++;
if (currDirection == SOUTH) pj--;
}
}
@ -318,6 +307,7 @@ void Robot::RobotLoop(void)
*/
if(chassis.CheckChassisTimer())
{
//Serial.println(lineSensor.CalcError());
// add synchronous, pre-motor-update actions here
if(robotState == ROBOT_LINING)
{
@ -330,6 +320,8 @@ void Robot::RobotLoop(void)
}
lineSensor.CalcError();
/**
* Check for any intersections
*/
@ -343,5 +335,15 @@ void Robot::RobotLoop(void)
{
HandleOrientationUpdate();
}
if (FindAprilTags()) HandleAprilTag(tag);
int32_t reading = 0;
if(loadCellHX1.GetReading(reading))
{
//Serial.print(millis());
//Serial.print('\t');
Serial.println(reading);
}
}

20
src/robot.h
View file

@ -4,6 +4,7 @@
#include <LSM6.h>
#include <openmv.h>
#include <servo32u4.h>
#include <HX711.h>
static float apriltag_Kp = 0.043; // rads per second per pixel
static float lining_kP2 = 0.67;
@ -54,6 +55,8 @@ protected:
Servo32U4Pin5 servo;
HX711<6, 13> loadCellHX1;
/* To add later: rangefinder, camera, etc.*/
uint16_t distanceTolerance = 65; //for width of tag
#define CENTER_THRESHOLD 10
@ -83,14 +86,15 @@ protected:
float turnRateDamp = 0.975; // the proportion of the turn rate retained each loop
float KTurnRate = 0.18; // slowdown coefficient
/**
* For tracking the motion of the Romi. We keep track of the intersection we came
* from and the one we're headed to. You'll program in the map in handleIntersection()
* and other functions.
*/
enum INTERSECTION {NODE_START, NODE_1, NODE_2, NODE_3,};
INTERSECTION nodeFrom = NODE_START;
INTERSECTION nodeTo = NODE_1;
uint8_t pi=0, pj=0;
int8_t currDirection = NORTH;
int8_t targetDirection = NORTH;
enum Direction {
NORTH = 1,
EAST = 0,
SOUTH = 3,
WEST = 2,
};
public:
Robot(void) {keyString.reserve(8);} //reserve some memory to avoid reallocation