Ejemplo n.º 1
0
// 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;
}
Ejemplo n.º 2
0
/* 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;
}
Ejemplo n.º 3
0
// 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];
	}
}
Ejemplo n.º 4
0
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);
}
Ejemplo n.º 5
0
// 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);
}
Ejemplo n.º 6
0
// 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;
    }
}
Ejemplo n.º 7
0
/* 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;
    }

}
Ejemplo n.º 8
0
// 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);
}
Ejemplo n.º 9
0
// 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);
    }
}