// report motor position and reset motor position (from Hall angular sensors)
// note motor_count is long (4 bytes) revs+frac rev
void cmdZeroPos(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame)
{ long motor_count[2];
motor_count[0] = pidObjs[0].p_state;
motor_count[1] = pidObjs[1].p_state;
radioConfirmationPacket(RADIO_DEST_ADDR, CMD_ZERO_POS,\
status, sizeof(motor_count), (unsigned char *)motor_count);
pidZeroPos(0); pidZeroPos(1);
}
unsigned char cmdZeroPos(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame) {
long motor_count[2];
motor_count[0] = pidObjs[0].p_state;
motor_count[1] = pidObjs[1].p_state;
radioSendData(RADIO_DST_ADDR, status, CMD_GET_AMS_POS, //TODO: Robot should respond to source of query, not hardcoded address
sizeof(motor_count), (unsigned char *)motor_count, 0);
pidZeroPos(0); pidZeroPos(1);
return 1;
}
unsigned char cmdZeroPos(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame, unsigned int src_addr) {
long motor_count[2];
motor_count[0] = pidObjs[0].p_state;
motor_count[1] = pidObjs[1].p_state;
radioSendData(src_addr, status, CMD_ZERO_POS,
sizeof(motor_count), (unsigned char *)motor_count, 0);
pidZeroPos(0);
pidZeroPos(1);
return 1;
}
