// returns the 12 bytes of the generated command packet
// remember to call delete on the returned array
byte* Command_Packet::GetPacketBytes()
{
byte* packetbytes= new byte[12];
// update command before calculating checksum (important!)
word cmd = Command;
command[0] = GetLowByte(cmd);
command[1] = GetHighByte(cmd);
word checksum = _CalculateChecksum();
packetbytes[0] = COMMAND_START_CODE_1;
packetbytes[1] = COMMAND_START_CODE_2;
packetbytes[2] = COMMAND_DEVICE_ID_1;
packetbytes[3] = COMMAND_DEVICE_ID_2;
packetbytes[4] = Parameter[0];
packetbytes[5] = Parameter[1];
packetbytes[6] = Parameter[2];
packetbytes[7] = Parameter[3];
packetbytes[8] = command[0];
packetbytes[9] = command[1];
packetbytes[10] = GetLowByte(checksum);
packetbytes[11] = GetHighByte(checksum);
return packetbytes;
}
/* InitToPose - gently moves Darwin into a ready position with speed 0x40
* Return - true if successful
* - false if unable to set torque, or if write fails
*/
bool DarwinController::InitToPose(){
unsigned char initpacket[108] = {0, };
unsigned char initparams[100] = {0, };
// Red eyes - Initialization in progress
int color = MakeColor(255, 0, 0);
unsigned char colorparams[2] = {GetLowByte(color), GetHighByte(color)};
MakePacket(initpacket, 0xC8, 2, 0x03, 0x1C, colorparams);
port.ClearPort();
port.WritePort(initpacket, 9);
// Torque enable
for(int IDnum = 0; IDnum < 21; IDnum++){
initparams[2*IDnum] = (unsigned char)(IDnum+1); // +1 because motors start at 1
initparams[2*IDnum+1] = 0x01;
}
unsigned char paramlength = 1;
unsigned char initnumparams = 40;
if(SyncWrite(initpacket, 0x18, initparams, initnumparams, paramlength) != 48){
printf("Failed Torque Enable in InitToPose!\n");
return false;
}
usleep(2000);
// Starting position and speed
for(int z = 0; z < 20; z++){
initparams[5*z] = z+1;
initparams[5*z+1] = 0x00;
initparams[5*z+2] = 0x08; // "zero" neutral position is 2048
initparams[5*z+3] = 0x40; // speed is 0x40
initparams[5*z+4] = 0x00;
}
if(SyncWrite(initpacket, 0x1E, initparams, 100, 4) != 108){
printf("Failed to init to pose\n");
return false;
}
sleep(1);
// Green eyes - Finished initializing
color = MakeColor(0, 255, 0);
initpacket[6] = GetLowByte(color);
initpacket[7] = GetHighByte(color);
colorparams[0] = GetLowByte(color);
colorparams[1] = GetHighByte(color);
MakePacket(initpacket, 0xC8, 2, 0x03, 0x1C, colorparams);
port.ClearPort();
port.WritePort(initpacket, 9);
return true;
}
// creates and parses a response packet from the finger print scanner
Response_Packet::Response_Packet(byte* buffer, bool UseSerialDebug)
{
CheckParsing(buffer[0], COMMAND_START_CODE_1, COMMAND_START_CODE_1, "COMMAND_START_CODE_1", UseSerialDebug);
CheckParsing(buffer[1], COMMAND_START_CODE_2, COMMAND_START_CODE_2, "COMMAND_START_CODE_2", UseSerialDebug);
CheckParsing(buffer[2], COMMAND_DEVICE_ID_1, COMMAND_DEVICE_ID_1, "COMMAND_DEVICE_ID_1", UseSerialDebug);
CheckParsing(buffer[3], COMMAND_DEVICE_ID_2, COMMAND_DEVICE_ID_2, "COMMAND_DEVICE_ID_2", UseSerialDebug);
CheckParsing(buffer[8], 0x30, 0x31, "AckNak_LOW", UseSerialDebug);
if (buffer[8] == 0x30) ACK = true; else ACK = false;
CheckParsing(buffer[9], 0x00, 0x00, "AckNak_HIGH", UseSerialDebug);
word checksum = CalculateChecksum(buffer, 10);
byte checksum_low = GetLowByte(checksum);
byte checksum_high = GetHighByte(checksum);
CheckParsing(buffer[10], checksum_low, checksum_low, "Checksum_LOW", UseSerialDebug);
CheckParsing(buffer[11], checksum_high, checksum_high, "Checksum_HIGH", UseSerialDebug);
Error = ErrorCodes::ParseFromBytes(buffer[5], buffer[4]);
ParameterBytes[0] = buffer[4];
ParameterBytes[1] = buffer[5];
ParameterBytes[2] = buffer[6];
ParameterBytes[3] = buffer[7];
ResponseBytes[0]=buffer[8];
ResponseBytes[1]=buffer[9];
for (int i=0; i < 12; i++)
{
RawBytes[i]=buffer[i];
}
}
int DarwinController::SetMoveSpeed(unsigned char joint_ID, int move_speed){
unsigned char speed_packet[9] = {0, };
unsigned char params[2] = {GetLowByte(move_speed), GetHighByte(move_speed)};
MakePacket(speed_packet, joint_ID, 2, WRITE, GOAL_POSITION_L, params);
unsigned char rxpacket[MAXNUM_RXPARAM] = {0, };
return ReadWrite(speed_packet, rxpacket);
}
// Converts an angle given in degrees into motor ticks and sends write packet to motor
int DarwinController::SetJointAngle(unsigned char joint_ID, int goal_angle){
unsigned char angle_packet[9] = {0, };
int goal_ticks = goal_angle * 11.378; // 4096/360
unsigned char params[2] = {GetLowByte(goal_ticks), GetHighByte(goal_ticks)};
MakePacket(angle_packet, joint_ID, 2, WRITE, GOAL_POSITION_L, params);
unsigned char rxpacket[MAXNUM_RXPARAM] = {0, };
return ReadWrite(angle_packet, rxpacket);
}
// TODO: Figure out range of reasonable speeds!
bool DarwinController::SetAllJointSpeeds(int speed){
unsigned char speedTxPacket[MAXNUM_TXPARAM];
unsigned char speedParams[60]; // 3 params each for 20 motors
unsigned char lowbyte = GetLowByte(speed);
unsigned char highbyte = GetHighByte(speed);
for(int i = 0; i < NUM_JOINTS; i++){
speedParams[3*i] = i+1;
speedParams[3*i+1] = lowbyte;
speedParams[3*i+2] = highbyte;
}
int result = SyncWrite(speedTxPacket, 0x20, speedParams, 60, 2);
if(result == 68){
return true;
} else {
return false;
}
}
/* WriteWord - Writes word out to the specified id and address
* Return - length of successfully read packet. Return -99999 if fails.
*/
int DarwinController::WriteWord(int id, int address, int value){
unsigned char txpacket[MAXNUM_TXPARAM + 10] = {0, };
txpacket[ID] = (unsigned char)id;
txpacket[INSTRUCTION] = WRITE;
txpacket[PARAMETER] = (unsigned char)address;
txpacket[PARAMETER+1] = (unsigned char)GetLowByte(value);
txpacket[PARAMETER+2] = (unsigned char)GetHighByte(value);
txpacket[LENGTH] = 5;
int txlength = txpacket[LENGTH] + 4;
FinishPacket(txpacket);
port.ClearPort();
int result = port.WritePort(txpacket, txlength);
if(result == txlength){ // do we want to return bool???
return result;
} else {
return -99999;
}
}
// if just goal changed, write 2 bytes
// if just gains chamnged write 3 bytes
// if both changed write all 5 bytes
void foo() {
// declare an array of joint data
JointData joints[NUM_JOINTS];
// motor 3 is enabled, has new goal position
joints[3].goal = 1027;
joints[3].flags = FLAG_ENABLE | FLAG_GOAL_CHANGED;
// later:
int packet_count = 0;
uint8_t change_flags = 0;
// figure out how many packets and what data gets sent
for (int i=0; i<NUM_JOINTS; ++i) {
const JointData& ji = joints[i];
bool enabled = ji.flags & FLAG_ENABLE; // pick off top bit to see if joint enabled
uint8_t changed = ji.flags & ~FLAG_ENABLE; // pick off bottom 7 bits
if (enabled && changed) { // if this motor is enabled and has new data
change_flags |= changed; // add in changes from this motor to set of all changes
packet_count += 1; // increment number of packets to send
}
}
// at this point can tell how many bytes per motor to send
uint8_t buf[MAX_PACKET_LENGTH*MAX_NUM_PACKETS + HEADER_LEN + FOOTER_LEN];
int buf_offset = 0;
unsigned char numparams = 0;
unsigned char lenparam;
unsigned char txpacket[128] = {0, };
// todo: fill in header
if (change_flags == FLAG_GOAL_CHANGED){ // If any of Goals changed but none of the PIDs changed
lenparam = 2;
for (int i=0; i<NUM_JOINTS; ++i) {
JointData& ji = joints[i];
if (ji.flags == FLAG_ENABLE + FLAG_GOAL_CHANGED){ // If this joint's Goal has changed, add to sync write
buf[buf_offset++] = i;
buf[buf_offset++] = GetLowByte(goal);
buf[buf_offset++] = GetHighByte(goal);
ji.flags = FLAG_ENABLE; // cleared the changed bits cause we are about to send this
numparams += 3;
}
}
}
else if (change_flags == FLAG_GAINS_CHANGED){ // If any of the gains changed but none of the Goals changed
lenparam = 3;
for (int i=0; i<NUM_JOINTS; ++i) {
JointData& ji = joints[i];
if (ji.flags == FLAG_ENABLE + FLAG_GAINS_CHANGED){ // If this joint's gains have changed, add to sync write
buf[buf_offset++] = i;
buf[buf_offset++] = ji.d;
buf[buf_offset++] = ji.i;
buf[buf_offset++] = ji.p;
ji.flags = FLAG_ENABLE; // cleared the changed bits cause we are about to send this
numparams += 4;
}
}
}
else if (change_flags == FLAG_GAINS_CHANGED + FLAG_GOAL_CHANGED){ // If both PID and Goal have changed
lenparam = 5;
for (int i=0; i<NUM_JOINTS; ++i) {
JointData& ji = joints[i];
if (ji.flags & FLAG_GOAL_CHANGED || ji.flags & FLAG_GAINS_CHANGED && ji.flags & FLAG_ENABLE){ // If this joint's Goal has changed, add to sync write
buf[buf_offset++] = i;
buf[buf_offset++] = ji.d;
buf[buf_offset++] = ji.i;
buf[buf_offset++] = ji.p;
buf[buf_offset++] = GetLowByte(goal);
buf[buf_offset++] = GetHighByte(goal);
ji.flags = FLAG_ENABLE; // cleared the changed bits cause we are about to send this
numparams += 6;
}
}
}
SyncWrite(txpacket, 0x03, buf, numparams, lenparam);
port->ClearPort();
port->WritePort(txpacket, numparams + 8);
}
// call this to write the changes in the JointData struct out to port
void DarwinController::Update_Motors(){
int packet_count = 0;
uint8_t change_flags = 0;
// figure out how many packets and what data gets sent
for (int i=1; i<NUM_JOINTS+1; ++i) {
const JointData& ji = joints[i];
// pick off top bit to see if joint enabled
bool enabled = ji.flags & FLAG_ENABLE;
// pick off bottom 7 bits
uint8_t changed = ji.flags & ~FLAG_ENABLE;
// if this motor is enabled and has new data
if (enabled && changed) {
// add in changes from this motor to set of all changes
change_flags |= changed;
// increment number of packets to send
packet_count += 1;
}
}
// at this point can tell how many bytes per motor to send
uint8_t buf[120] = {0, };
int buf_offset = 0;
unsigned char numparams = 0;
unsigned char lenparam;
unsigned char txpacket[128] = {0, };
unsigned char inst;
// If any of Goals changed but none of the PIDs changed
if (change_flags == FLAG_GOAL_CHANGED){
lenparam = 2;
inst = 0x1E; //starting address for goal position Low
for (int i=0; i<NUM_JOINTS+1; ++i) {
JointData& ji = joints[i];
// If this joint's Goal has changed, add to sync write
if (ji.flags == FLAG_ENABLE + FLAG_GOAL_CHANGED){
buf[buf_offset++] = i;
buf[buf_offset++] = GetLowByte(ji.goal);
buf[buf_offset++] = GetHighByte(ji.goal);
// cleared the changed bits cause we are about to send this
ji.flags = FLAG_ENABLE;
numparams += 3;
}
}
}
// If any of the gains changed but none of the Goals changed
else if (change_flags == FLAG_GAINS_CHANGED){
lenparam = 3;
inst = 0x1A; // starting address for pid gains
for (int i=0; i<NUM_JOINTS+1; ++i) {
JointData& ji = joints[i];
// If this joint's gains have changed, add to sync write
if (ji.flags == FLAG_ENABLE + FLAG_GAINS_CHANGED){
buf[buf_offset++] = i;
buf[buf_offset++] = ji.d;
buf[buf_offset++] = ji.i;
buf[buf_offset++] = ji.p;
// cleared the changed bits cause we are about to send this
ji.flags = FLAG_ENABLE;
numparams += 4;
}
}
}
// If both PID and Goal have changed
else if (change_flags == FLAG_GAINS_CHANGED + FLAG_GOAL_CHANGED){
lenparam = 6;
inst = 0x1A; //starting address for pid gains
for (int i=0; i<NUM_JOINTS+1; ++i) {
JointData& ji = joints[i];
// If this joint's Goal has changed, add to sync write
if (ji.flags & FLAG_GOAL_CHANGED || ji.flags & FLAG_GAINS_CHANGED && ji.flags & FLAG_ENABLE){
buf[buf_offset++] = i;
buf[buf_offset++] = ji.d;
buf[buf_offset++] = ji.i;
buf[buf_offset++] = ji.p;
buf[buf_offset++] = 0x00; // I hate everything
buf[buf_offset++] = GetLowByte(ji.goal); // shhh it's ok
buf[buf_offset++] = GetHighByte(ji.goal);
// cleared the changed bits cause we are about to send this
ji.flags = FLAG_ENABLE;
numparams += 7;
}
}
}
if(packet_count){
SyncWrite(txpacket, inst, buf, numparams, lenparam);
}
}
