CommandVector commands() const {
CommandVector cmds;
cmds.push_back(boost::make_shared<BackupStartCommand>());
cmds.push_back(boost::make_shared<BackupThrottleCommand>());
cmds.push_back(boost::make_shared<BackupStatusCommand>());
return cmds;
}
// this version just replaces everything
void KovanModule::writeDigitals(const unsigned char &values, const unsigned char &pullups, const unsigned char &outputEnables)
{
std::cout<<"Warning: writeDigitals has not been tested." << std::endl; // TODO
Command c0 = createWriteCommand(DIG_OUT, values);
Command c1 = createWriteCommand(DIG_OUT_ENABLE, outputEnables);
Command c2 = createWriteCommand(DIG_PULLUPS, pullups);
CommandVector commands;
commands.push_back(c0);
commands.push_back(c1);
commands.push_back(c2);
// annoying that we have to do this
Command r0;
r0.type = StateCommandType;
commands.push_back(r0);
send(commands);
State state;
// shouldn't need this
if(!recv(state)) {
std::cout << "Error: didn't get state back!" << std::endl;
return;
}
singleWrite(DIG_UPDATE_T, 1);
singleWrite(DIG_UPDATE_T, 0);
singleWrite(DIG_SAMPLE_T, 0); // FPGA captures new dig values
singleWrite(DIG_SAMPLE_T, 1);
}
int KovanModule::singleWrite(const unsigned short &address, const unsigned short &value)
{
Command c0 = createWriteCommand(address, value);
CommandVector commands;
commands.push_back(c0);
// annoying that we have to do this
Command r0;
r0.type = StateCommandType;
commands.push_back(r0);
send(commands);
State state;
// shouldn't need this
if(!recv(state)) {
std::cout << "Error: didn't get state back!" << std::endl;
return -1;
}
return 0;
}
void KovanModule::turnMotorsOff()
{
// speed 0
Command c0 = createWriteCommand(MOTOR_PWM_0, 0);
Command c1 = createWriteCommand(MOTOR_PWM_1, 0);
Command c2 = createWriteCommand(MOTOR_PWM_2, 0);
Command c3 = createWriteCommand(MOTOR_PWM_3, 0);
// passive brake
Command c4 = createWriteCommand(MOTOR_DRIVE_CODE_T, 0);
CommandVector commands;
commands.push_back(c0);
commands.push_back(c1);
commands.push_back(c2);
commands.push_back(c3);
commands.push_back(c4);
// annoying that we have to do this
Command r0;
r0.type = StateCommandType;
commands.push_back(r0);
send(commands);
State state;
// shouldn't need this
if(!recv(state)) {
std::cout << "Error: didn't get state back!" << std::endl;
return;
}
}
void KovanModule::moveServo(const char &servoNum, const unsigned short &position)
{
unsigned short val = (unsigned int)(((SERVO_MAX - SERVO_MIN) * (position / 1024.0)) + SERVO_MIN) >> 8;
// we are only writing to the upper 16 bits of the 24 bit register .....
unsigned short addy = 0;
switch(servoNum){
case 0:
addy = SERVO_COMMAND_0;
break;
case 1:
addy = SERVO_COMMAND_1;
break;
case 2:
addy = SERVO_COMMAND_2;
break;
case 3:
addy = SERVO_COMMAND_3;
break;
default:
std::cout << "Invalid servo number" << std::endl;
return;
}
Command c0;
c0.type = StateCommandType;
// servo position (pwm)
Command c1 = createWriteCommand(addy, val);
// Servo PWM period only has to be set once
// value is computed as 0x03F7A0
// but we only have access to upper 16 bits
//Command c2 = createWriteCommand(SERVO_PWM_PERIOD_T, 0x03F7);
CommandVector commands;
commands.push_back(c0);
commands.push_back(c1);
State state;
send(commands);
if(!recv(state)) {
std::cout << "Error: didn't get state back!" << std::endl;
return;
}
}
const CommandPtr Find ( const std::string& name ) {
CommandVector::const_iterator iter = commands_.begin();
while ( iter != commands_.end() ) {
if ( name == ( *iter )->Name() ) break;
++iter;
}
// check for an invalid command and throw
if ( iter == commands_.end() )
throw std::runtime_error ( "Invalid command request" );
return *iter;
}
bool KovanModule::send(const CommandVector& commands)
{
uint32_t packetSize = 0;
Packet *packet = createPacket(commands.size(), packetSize);
memcpy(packet->commands, &commands[0], commands.size() * sizeof(Command));
bool ret = true;
if(sendto(m_sock, packet, packetSize, 0, (sockaddr *)&m_out, sizeof(m_out)) != packetSize) {
perror("sendto");
ret = false;
}
free(packet);
return ret;
}
void translateIdxBuildCommandsToBuildCommands(
const IdxCommandVector& icommands,
const ComponentGraph& cg,
CommandVector& commands) {
// get components in order of iteration into vector
std::vector<Component> indexableComps;
{
ComponentGraph::ComponentIterator cit, cit_end;
for(boost::tie(cit, cit_end) = cg.getComponents();
cit != cit_end; ++cit) {
indexableComps.push_back(*cit);
}
}
BOOST_FOREACH(const IdxBuildCommand& ibc, icommands) {
BuildCommand bc;
BOOST_FOREACH(unsigned idx, ibc.collapse) {
assert(idx < indexableComps.size() && "got bad component index");
bc.collapse.push_back(indexableComps[idx]);
}
BOOST_FOREACH(unsigned idx, ibc.share) {
assert(idx < indexableComps.size() && "got bad component index");
bc.share.push_back(indexableComps[idx]);
}
commands.push_back(bc);
}
int KovanModule::getState(State &state)
{
Command c0;
c0.type = StateCommandType;
CommandVector commands;
commands.push_back(c0);
send(commands);
if(!recv(state)) {
std::cout << "Error: didn't get state back!" << std::endl;
return -1;
}
return 0;
}
// test to turn all 4 motors on
//
// pwm_div is probably constant
//
// pwm_val depends on the speed (shared for all motors currently)
//
// set speedPercent 0-100
//
// drive_code specifies forward/reverse/idle/brake
// Forward = 10, Reverse = 01, Brake = 11, Idle = 00
void KovanModule::turnMotorsOn(const unsigned short &speedPercent)
{
unsigned short speed = 0;
if (speedPercent > 100){
speed = 2600;
}else{
speed = (speedPercent*2600) / 100;
}
Command c0 = createWriteCommand(MOTOR_PWM_0, speed);
Command c1 = createWriteCommand(MOTOR_PWM_1, speed);
Command c2 = createWriteCommand(MOTOR_PWM_2, speed);
Command c3 = createWriteCommand(MOTOR_PWM_3, speed);
// drive code (all forward)
Command c4 = createWriteCommand(MOTOR_DRIVE_CODE_T, 0xAA);
CommandVector commands;
commands.push_back(c0);
commands.push_back(c1);
commands.push_back(c2);
commands.push_back(c3);
commands.push_back(c4);
// annoying that we have to do this
Command r0;
r0.type = StateCommandType;
commands.push_back(r0);
send(commands);
State state;
// shouldn't need this
if(!recv(state)) {
std::cout << "Error: didn't get state back!" << std::endl;
return;
}
}
void KovanModule::speedTest()
{
Command c0;
c0.type = StateCommandType;
Command c1 = createWriteCommand(MOTOR_PWM_0, 3);
CommandVector commands;
commands.push_back(c0);
commands.push_back(c1);
State state;
for (int i = 0; i < 1000; i++){
send(commands);
if(!recv(state)) {
std::cout << "Error: didn't get state back!" << std::endl;
return;
}
}
}
void Add ( CommandPtr commandPtr ) {
commands_.push_back ( commandPtr );
}
