Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add support for the original SCI protocol #25

Merged
merged 1 commit into from Aug 23, 2016

Conversation

@lopsided98
Copy link
Contributor

commented Jul 14, 2016

This PR adds support for the original SCI protocol used by the Discovery and 400 series (though apparently not all of them) Roombas.

Since three robots are now supported, the existing system of specifying which model to use did not work that well. Therefore, I decided to create a new RobotModel class, which encapsulates the the wheelbase width, baud rate, and protocol version, as well as any other parameters that might differ between models.

I added a new ProtocolVersion enum which largely replaces the old RobotModel enum in the various conditionals that decide how to communicate with the robot. The three protocol versions are based on the three documents that describe them:

Like we talked about, I split the Serial class into SerialStream and a new SerialQuery class which uses the Sensors command to retrieve data.

Since V_1 does not include the "drive direct" command, and this is a very important command for create_autonomy, I decided to emulate it using the "drive" command.

I also had to emulate the mode retrieval and requested wheel velocities because they are not supported in V_1. I decided to not request the requested velocity sensor packets (even when they are supported) since the exact same functionality can be gained by just caching the last requested velocities in driveWheels().

I found that the odometry on my Roomba 400 is much better than on my Create because the angle field is implemented as "the right wheel distance minus the left wheel
distance, divided by two" and it does not have the same bug as the angle field on the Create 1.

I tested this on my Roomba and my Create, but there is a possibility that I broke the odometry on the Create 2, so I would recommend testing it.

Let me know what you think. I will have a PR for create_autonomy soon that supports these changes.

ADD_PACKET(ID_CLIFF_RIGHT, 1, "cliff_right", V_ALL);
ADD_PACKET(ID_VIRTUAL_WALL, 1, "virtual_wall", V_ALL);
ADD_PACKET(ID_OVERCURRENTS, 1, "overcurrents", V_1);
ADD_PACKET(ID_DIRT_DETECT_LEFT, 1, "dirt_detect_left", V_1 | V_2);

This comment has been minimized.

Copy link
@jacobperron

jacobperron Jul 25, 2016

Member

I think for DIRT_DETECT_LEFT it should be V_ALL

#include "create/types.h"

namespace create {
RobotModel::RobotModel(const std::string& name) {

This comment has been minimized.

Copy link
@jacobperron

jacobperron Jul 25, 2016

Member

What if we change the signature to RobotModel(const RobotModel& model) instead? This way we don't need the ugly if-else block below with the error throwing.

This comment has been minimized.

Copy link
@lopsided98

lopsided98 Jul 28, 2016

Author Contributor

Maybe I am misunderstanding you, but I'm not sure how this would work. This constructor is used to convert a string into a RobotModel, which is used to implement the parsing of the robot_model parameter in create_autonomy.

I was basically trying to implement similar behavior to the Java Enum.valueOf() method, and while I could do this with a lookup table, it seems overkill for this situation.

This comment has been minimized.

Copy link
@jacobperron

jacobperron Jul 28, 2016

Member

In order to prevent invalid strings being passed into the constructor, I was suggesting to make it a copy constructor instead.

But, as it stands we only expect the three robot models defined on lines 44-46 to be used. Therefore, I think we can safely remove this constructor altogether.

This comment has been minimized.

Copy link
@lopsided98

lopsided98 Jul 29, 2016

Author Contributor

How should the parameter be parsed then?

This comment has been minimized.

Copy link
@jacobperron

jacobperron Jul 29, 2016

Member

By removing the public constructor we should only use the three models already defined. For example:

// Using a ROOMBA_400 model
RobotModel model0 = RobotModel::ROOMBA_400;

// Or a Create 1 model
RobotModel model1 = RobotModel::CREATE_1;

// This is not allowed!
RobotModel model2("CREATE_1");
}

bool RobotModel::operator ==(RobotModel& other) {
return id = other.id;

This comment has been minimized.

Copy link
@jacobperron

jacobperron Jul 25, 2016

Member

I think you meant to use ==

bool operator==(RobotModel& other);
operator uint32_t();

static RobotModel ROOMBA_400; // Roomba 400 series

This comment has been minimized.

Copy link
@jacobperron

jacobperron Jul 25, 2016

Member

It would be nice if these were const, but I can't think of an elegant way to enforce it. The only thing I can think of is to make these private and provide getter functions for them.


class RobotModel {
public:
uint32_t id;

This comment has been minimized.

Copy link
@jacobperron

jacobperron Jul 25, 2016

Member

Perhaps all of these member variables are best kept private and we use getters to access them, to enforce the use of the defined models below. What do you think?

@@ -271,24 +299,32 @@ namespace create {
//}

bool Create::setMode(const CreateMode& mode) {
// Switch to safe mode (required for compatibility with V_1)
if (!(serial->sendOpcode(OC_START) && serial->sendOpcode(OC_CONTROL))) return false;

This comment has been minimized.

Copy link
@jacobperron

jacobperron Jul 25, 2016

Member

Can this just be conditional on if model.version == V_1 ?

break;
case MODE_PASSIVE:
return serial->sendOpcode(OC_START);
break;
case MODE_SAFE:
return serial->sendOpcode(OC_SAFE);
// Already in safe mode

This comment has been minimized.

Copy link
@jacobperron

jacobperron Jul 25, 2016

Member

Change this back if line 303 changes.

}
return false;
this->mode = mode;

This comment has been minimized.

Copy link
@jacobperron

jacobperron Jul 25, 2016

Member

Since each case has a return, this statement is never executed.

float boundedLeftVel = BOUND_CONST(leftVel, -model.maxVelocity, model.maxVelocity);
float boundedRightVel = BOUND_CONST(rightVel, -model.maxVelocity, model.maxVelocity);
requestedLeftVel = boundedLeftVel;
requestedRightVel = boundedRightVel;

This comment has been minimized.

Copy link
@jacobperron

jacobperron Jul 25, 2016

Member

We should document that the requested velocities have been bounded by the maximum of the model in the header line 461 and 465

} else {
float radius;
// Prevent divide by zero when driving straight
if (boundedLeftVel != boundedRightVel) {

This comment has been minimized.

Copy link
@jacobperron

jacobperron Jul 25, 2016

Member

A better way to compare the floating point values: fabs(boundedLeftVel - boundedRightVel) > EPS)

This comment has been minimized.

Copy link
@lopsided98

lopsided98 Jul 27, 2016

Author Contributor

In this case, I'm pretty sure it doesn't actually matter which method is used as I only care if the left and right speeds are exactly equal because this would cause a divide by 0 on line 393. As long as they are the tiniest bit different, the normal math should work.

I can change it anyway, though, if you want.

This comment has been minimized.

Copy link
@jacobperron

jacobperron Jul 27, 2016

Member

True, but I think I prefer it changed. A difference of epsilon can result in a really large radius which will result in driving straight anyways, which can be directly set in the else case.

BOUND(rightCmd, -util::CREATE_2_MAX_VEL * 1000, util::CREATE_2_MAX_VEL * 1000);
bool Create::driveWheels(const float& leftVel, const float& rightVel) {
float boundedLeftVel = BOUND_CONST(leftVel, -model.maxVelocity, model.maxVelocity);
float boundedRightVel = BOUND_CONST(rightVel, -model.maxVelocity, model.maxVelocity);

This comment has been minimized.

Copy link
@jacobperron

float vel;
// Fix signs for spin in place
if (boundedLeftVel == -boundedRightVel || std::abs(roundf(radius * 1000)) <= 1) {

This comment has been minimized.

Copy link
@jacobperron

jacobperron Jul 25, 2016

Member

Again, take care when comparing floating points this way. I think you want if (boundedLeftVel - boundedRightVel < EPS)

@jacobperron

This comment has been minimized.

Copy link
Member

commented Jul 25, 2016

Nice work
👍 for RobotModel and separate serial classes.

I'll try it out on the Create 2 and let you know how it goes.

@jacobperron

This comment has been minimized.

Copy link
Member

commented Jul 25, 2016

Tried out the changes on a Create 1 and a Create 2. Everything seems to be OK 👍

@jacobperron

This comment has been minimized.

Copy link
Member

commented Jul 25, 2016

@lopsided98 Could you please add a brief section to the README with links to the three supported protocols? Thanks.

@jacobperron

This comment has been minimized.

Copy link
Member

commented Aug 8, 2016

Thanks, if you squash your commits I'll merge 👍

@jacobperron jacobperron merged commit 2297d1d into AutonomyLab:master Aug 23, 2016

1 check passed

continuous-integration/travis-ci/pr The Travis CI build passed
Details
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
2 participants
You can’t perform that action at this time.