ank/RBE2002
1
Fork 0
Code Issues Activity

Compare commits

base: ank:95197007d0f2d8b29c495e5633121204acafd6be
Branches Tags
ank:main
ank:benni-stash
ank:lab2
..
compare: ank:4faf4986cf4db3378dbe297eda4a969bf210ab0d
Branches Tags
ank:main
ank:benni-stash
ank:lab2

No commits in common. "95197007d0f2d8b29c495e5633121204acafd6be" and "4faf4986cf4db3378dbe297eda4a969bf210ab0d" have entirely different histories.
95197007d0 ... 4faf4986cf

9 changed files with 73 additions and 124 deletions
Show stats Expand all files Collapse all files

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

@ -155,11 +155,4 @@ 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,7 +39,6 @@ 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,8 +133,6 @@ protected:
currentSetpoint += deltaSetpoint;
float error = currentSetpoint - speed;
//Serial.println(speed);
//Serial.println(targetSpeed);
sumError += error;
@ -167,13 +165,13 @@ protected:
if(debug)
{
Serial.print("");
Serial.print("target:");
Serial.print(targetSpeed);
Serial.print("\t");
Serial.print("\tspeed:");
Serial.print(speed);
Serial.print("\t");
Serial.print("\terror:");
Serial.print(error);
Serial.print("\t");
Serial.print("\teffort:");
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,11 +22,8 @@ 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__TRACK_DEBUG__
; -DDARK
; -D__IMU_DEBUG__

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
blinkTagId = tag.id;
tagId = tag.id;
}
Serial.print(F("Tag [cx="));
Serial.print(tag.cx);

29
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();
@ -95,29 +95,6 @@ void Robot::HandleKeyCode(int16_t keyCode)
case ENTER_SAVE:
chassis.SetTwist(0, 0);
break;
case REWIND:
servo.setTargetPos(keyString.toInt());
Serial.println(keyString);
keyString = "";
break;
case NUM_1:
case NUM_2:
case NUM_3:
keyString += (char)(keyCode + 33);
break;
case NUM_4:
case NUM_5:
case NUM_6:
keyString += (char)(keyCode + 32);
break;
case NUM_7:
case NUM_8:
case NUM_9:
keyString += (char)(keyCode + 31);
break;
case NUM_0_10:
keyString += '0';
break;
}
}
@ -132,7 +109,6 @@ 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;
@ -145,7 +121,6 @@ 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;
@ -204,4 +179,4 @@ void Robot::EnterSetupMode(void)
{
Serial.println("-> SETUP");
robotCtrlMode = CTRL_SETUP;
}
}

116
src/robot.cpp
View file

@ -23,10 +23,6 @@ void Robot::InitializeRobot(void)
// The line sensor elements default to INPUTs, but we'll initialize anyways, for completeness
lineSensor.Initialize();
servo.attach();
loadCellHX1.Init();
}
void Robot::EnterIdleState(void)
@ -85,8 +81,6 @@ 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
@ -119,42 +113,16 @@ 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(millis());
//Serial.print('\t');
//Serial.print(imu.gyroBias.z);
//Serial.print('\t');
//Serial.print(eulerAngles.z);
//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);
//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);
Serial.println(imu.g.z);
#endif
}
@ -185,9 +153,9 @@ void Robot::LineFollowingUpdate(void)
float setpoint = 3.5;
#endif
float lineError = setpoint - lineSensor.CalcError();
float powErr = (lineError*lineError);
powErr = (lineError < 0 ? -powErr : powErr);
@ -276,14 +244,55 @@ void Robot::LineFollowingUpdate(void)
*/
void Robot::HandleIntersection(void)
{
//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--;
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');
}
}
@ -294,8 +303,6 @@ void Robot::RobotLoop(void)
* and asynchronous events (IR presses, distance readings, etc.).
*/
servo.update();
/**
* Handle any IR remote keypresses.
*/
@ -307,7 +314,6 @@ void Robot::RobotLoop(void)
*/
if(chassis.CheckChassisTimer())
{
//Serial.println(lineSensor.CalcError());
// add synchronous, pre-motor-update actions here
if(robotState == ROBOT_LINING)
{
@ -320,8 +326,6 @@ void Robot::RobotLoop(void)
}
lineSensor.CalcError();
/**
* Check for any intersections
*/
@ -335,15 +339,5 @@ 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);
}
}

23
src/robot.h
View file

@ -3,8 +3,6 @@
#include <LineSensor.h>
#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;
@ -53,10 +51,6 @@ protected:
/* Line sensor */
LineSensor lineSensor;
Servo32U4Pin5 servo;
HX711<6, 13> loadCellHX1;
/* To add later: rangefinder, camera, etc.*/
uint16_t distanceTolerance = 65; //for width of tag
#define CENTER_THRESHOLD 10
@ -86,15 +80,14 @@ protected:
float turnRateDamp = 0.975; // the proportion of the turn rate retained each loop
float KTurnRate = 0.18; // slowdown coefficient
uint8_t pi=0, pj=0;
int8_t currDirection = NORTH;
int8_t targetDirection = NORTH;
enum Direction {
NORTH = 1,
EAST = 0,
SOUTH = 3,
WEST = 2,
};
/**
* 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;
public:
Robot(void) {keyString.reserve(8);} //reserve some memory to avoid reallocation