int record_data(FILE* file)
{
int i;
char cmd[256];
long mtime, seconds, useconds;
static int position[4]={0,0,0,0};
static double velocity[4]={0,0,0,0};
static double current[4]={0,0,0,0};
gettimeofday(&end, NULL);
seconds = end.tv_sec - start.tv_sec;
useconds = end.tv_usec - start.tv_usec;
mtime = long((seconds*1000 + useconds/1000.0) + 0.5);
for (i = 0; i < 4; i++){
sprintf(cmd, "TP%c", 'A'+i);
position[i] = g.commandValue(cmd);
sprintf(cmd, "TV%c", 'A'+i);
velocity[i] = g.commandValue(cmd);
sprintf(cmd, "TT%c", 'A'+i);
current[i] = g.commandValue(cmd);
}
if (file < 0) return RTN_ERROR;
fprintf(file, "%ld %d %d %d %d %d %d %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g\n", mtime, pCoreStatus->imu.state,
pCoreStatus->imu.act_jnt,
position[0], position[1], position[2], position[3],
velocity[0], velocity[1], velocity[2], velocity[3],
current[0], current[1], current[2], current[3],
pCoreStatus->imu.current_YPR[0], pCoreStatus->imu.current_YPR[1], pCoreStatus->imu.current_YPR[2],
pCoreStatus->imu.reference_YPR[0], pCoreStatus->imu.reference_YPR[1], pCoreStatus->imu.reference_YPR[2],
pCoreStatus->imu.delta_YPR[0], pCoreStatus->imu.delta_YPR[1], pCoreStatus->imu.delta_YPR[2]);
return RTN_OK;
}
int start_galildmc()
{
g.programDownloadFile("./nasal_robot.dmc");
g.command("XQ#BEGIN");
printf("start gailidmc\n");
return RTN_OK;
}
int stop_galildmc()
{
g.command("v1=0;v2=0;v3=0;v4=0");
g.command("OF 0,0,0,0");
g.command("ST");
g.command("XQ#STOP");
printf("stop galildmc\n");
return RTN_OK;
}
int axis_multi_jog_cmd(unsigned int mark, double* vel)
{
char str[256], st[64];
int i, n=0;
str[0] = 0;
for (i = 0; i < 4; i++) {
if (mark & (1<<i)) {
sprintf(st, "v%d=%.02f;", i+1, vel[i]);
n += sprintf(&str[n], "%s", st);
}
}
if (n == 0) {
printf("axis_jog_cmd : the axis number error \n");
return RTN_ERROR;
}
str[n]=0;
if (memcmp(ostr, str, n) != 0){
printf("axis_jog_cmd : %s\n", str);
strcpy(ostr, str);
}
g.command(str);
return RTN_OK;
}
// ASSUMES 'g' IS VALID and points to Galil object
void control_IGV(Galil &g, XINPUT_STATE &prev_state){
// LEFT_THUMB_DEADZONE; // experiemental value(anywhere from 0-65534)
// GALIL SPECS:
string RIGHT_MOTOR_JGPOWER_str = "";
string LEFT_MOTOR_JGPOWER_str = "";
int RIGHT_MOTOR_ACCELERATION = 25000;
int RIGHT_MOTOR_DECCELERATION = 45000;
int RIGHT_MOTOR_JGPOWER_MAX = 55000; //definitely should not use that..
int LEFT_MOTOR_ACCELERATION = RIGHT_MOTOR_ACCELERATION;
int LEFT_MOTOR_DECCELERATION = RIGHT_MOTOR_DECCELERATION;
int LEFT_MOTOR_JGPOWER_MAX = RIGHT_MOTOR_JGPOWER_MAX;
g.commandValue("AC ," + toString(RIGHT_MOTOR_ACCELERATION) + "; "
+ "DC ," + toString(RIGHT_MOTOR_DECCELERATION) + "; "
+ "JG 0,0;");
g.commandValue("BG;"); // BEGIN THE GALIL EXECUTION
XINPUT_STATE state;
ZeroMemory( &state, sizeof(XINPUT_STATE) );
DWORD dwresult = XInputGetState(0, &state);
float LX;
float LY;
float magnitude;
float normalizedLX;
float normalizedLY;
float normalizedMagnitude;
int cur_rightPower = 0;
int cur_leftPower = 0;
int prev_rightPower = 0;
int prev_leftPower = 0;
while(state.Gamepad.wButtons != XINPUT_GAMEPAD_BACK){
LX = state.Gamepad.sThumbLX;
LY = state.Gamepad.sThumbLY;
magnitude = sqrt(LX*LX + LY*LY);
normalizedLX = LX/magnitude;
normalizedLY = LY/magnitude;
// tune X's and Y's for the Deadzones
if(abs(LX) < LEFT_THUMB_DEADZONE){
LX = 0;
}
else{
LX -= LEFT_THUMB_DEADZONE;
}
if(abs(LY) < LEFT_THUMB_DEADZONE){
LY = 0;
}
else{
LY -= LEFT_THUMB_DEADZONE;
}
RIGHT_MOTOR_JGPOWER_str = toString(cur_rightPower = int((LY/LY_MAX) * (RIGHT_MOTOR_JGPOWER_MAX) - (LX/LX_MAX) * (LEFT_MOTOR_JGPOWER_MAX)));
LEFT_MOTOR_JGPOWER_str = toString(cur_leftPower = int((LY/LY_MAX) * (RIGHT_MOTOR_JGPOWER_MAX) + (LX/LX_MAX) * (LEFT_MOTOR_JGPOWER_MAX)));
if(magnitude > LEFT_THUMB_DEADZONE){ // then we want to consider what it does
if(magnitude > THUMB_MAX) // max thumbing
magnitude = THUMB_MAX;
// make sure the magnitude is the actual magnitude of that i need to consider
magnitude -= LEFT_THUMB_DEADZONE;
normalizedMagnitude = magnitude/(THUMB_MAX - LEFT_THUMB_DEADZONE); // this is a percentage of the possible strength of the magnitude
// RIGHT MOTOR TEST WITH MAGNITUDE OF THUMB STICK MOVEMENT (NOT NEEDED ANYMORE.. USING COMPONENTS LX and LY to DETERMINE MOVEMENT.
//RIGHT_MOTOR_JGPOWER_str = toString(RIGHT_MOTOR_JGPOWER_MAX * normalizedMagnitude);
//cout << RIGHT_MOTOR_JGPOWER_str << endl;
//cout << g.commandValue("MG @ABS[-2.37]; MG " + RIGHT_MOTOR_JGPOWER_str + ";") << endl;
}
else{
magnitude = 0;
normalizedMagnitude = 0;
}// end DEADZONE CHECK
// now i can send the motor commands accordingly
// if the power to left or right is different then send it to galil.. else dont send anything:
if((cur_rightPower != prev_rightPower) || (cur_leftPower != cur_leftPower)){
g.commandValue("JG " + LEFT_MOTOR_JGPOWER_str + "," + RIGHT_MOTOR_JGPOWER_str + ";");
cout << "JG " + LEFT_MOTOR_JGPOWER_str + "," + RIGHT_MOTOR_JGPOWER_str + ";" << endl;
}
prev_leftPower = cur_leftPower;
prev_rightPower = cur_rightPower;
if(STOP_MOTION == true){
break;
}
dwresult = XInputGetState(0, &state); // to get status and next state..
}// end while
g.commandValue("ST;");
cout << "no longer controlling IGV with controller" << endl;
return;
}
