void ForceTorqueSensor::setPuck(Puck* puck)
{
// Call super
SpecialPuck::setPuck(puck);
p->wake();
bus = &p->getBus();
id = p->getId();
// TODO(dc): Fix this once FT sensors have working ROLE/VERS properties.
propId = Puck::getPropertyId(Puck::FT, Puck::PT_ForceTorque, 0);
tare();
}
void init_YEI()
{
// Serial Port Configuration for YEI
Serial2.begin(BAUD); // :231,460800
delay(10);
// Clear Serial 2 for IMU Read
Serial2.flush();
while (Serial2.available())
Serial2.read();
delay(10);
// Read IMU Baud Rate
IMU_Rd_command(232, 1, NOT_BYTE, &dummy);
Serial.print("IMU Baudrate:");
Serial.println(dummy.ival);
delay(10);
//Auto-Calibrate IMU gyroscopes
//IMU_Gyro_Calibrate();
//delay(5000);
//Configure IMU LED Color
IMUsetColor(1, 0, 0);
// IMUreadColor();
IMU_Rd_command(43, 1, NOT_BYTE, &temp);
// Get Some Variables
Serial.print("Temperature ");
Serial.println(temp.fval);
Serial.println("Setting up streaming");
IMU_Send_command(80, 8, IS_BYTE, (union SPIdata*) streamChannel);
delay(1);
IMU_Rd_command(81, 8, IS_BYTE, (union SPIdata*) streamChannel1);
for (int k = 0; k < 7; k++)
{
Serial.print(streamChannel1[k]);
Serial.print(" ");
}
Serial.println(streamChannel1[7]);
IMU_Send_command(221, 1, NOT_BYTE, (union SPIdata*) &header);
IMU_Send_command(82, 3, NOT_BYTE, (union SPIdata*) streamTimes);
IMU_Rd_command(83, 3, NOT_BYTE, streamTimes1);
for (int k = 0; k < 2; k++) {
Serial.print(streamTimes1[k].ival);
Serial.print(" ");
}
Serial.println(streamTimes1[2].ival);
tare();
delay(1);
Serial.println("Initialized YEI IMU.");
IMU_Send_command(95, 1, NOT_BYTE, (union SPIdata*) &init_timestamp); // reset timestamp
IMUstart_streaming(); // Start streaming
delay(100);
// Clear Serial2 for IMU Read
Serial2.flush();
while (Serial2.available())
Serial2.read();
}
