void scara_set_axis_is_at_home(const AxisEnum axis) {
if (axis == Z_AXIS)
current_position[Z_AXIS] = Z_HOME_POS;
else {
/**
* SCARA homes XY at the same time
*/
float homeposition[XYZ];
LOOP_XYZ(i) homeposition[i] = base_home_pos((AxisEnum)i);
// SERIAL_ECHOLNPAIR("homeposition X:", homeposition[X_AXIS], " Y:", homeposition[Y_AXIS]);
/**
* Get Home position SCARA arm angles using inverse kinematics,
* and calculate homing offset using forward kinematics
*/
inverse_kinematics(homeposition);
forward_kinematics_SCARA(delta[A_AXIS], delta[B_AXIS]);
// SERIAL_ECHOLNPAIR("Cartesian X:", cartes[X_AXIS], " Y:", cartes[Y_AXIS]);
current_position[axis] = cartes[axis];
update_software_endstops(axis);
}
}
inline void _O2 ubl_buffer_segment_raw(const float (&in_raw)[XYZE], const float &fr) {
#if ENABLED(SKEW_CORRECTION)
float raw[XYZE] = { in_raw[X_AXIS], in_raw[Y_AXIS], in_raw[Z_AXIS] };
planner.skew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]);
#else
const float (&raw)[XYZE] = in_raw;
#endif
#if ENABLED(DELTA) // apply delta inverse_kinematics
DELTA_IK(raw);
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], fr, active_extruder);
#elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw)
inverse_kinematics(raw); // this writes delta[ABC] from raw[XYZE]
// should move the feedrate scaling to scara inverse_kinematics
const float adiff = ABS(delta[A_AXIS] - scara_oldA),
bdiff = ABS(delta[B_AXIS] - scara_oldB);
scara_oldA = delta[A_AXIS];
scara_oldB = delta[B_AXIS];
float s_feedrate = MAX(adiff, bdiff) * scara_feed_factor;
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], s_feedrate, active_extruder);
#else // CARTESIAN
planner.buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], in_raw[E_AXIS], fr, active_extruder);
#endif
}
void report_current_position_detail() {
stepper.synchronize();
SERIAL_PROTOCOLPGM("\nLogical:");
report_xyze(current_position);
SERIAL_PROTOCOLPGM("Raw: ");
const float raw[XYZ] = { RAW_X_POSITION(current_position[X_AXIS]), RAW_Y_POSITION(current_position[Y_AXIS]), RAW_Z_POSITION(current_position[Z_AXIS]) };
report_xyz(raw);
SERIAL_PROTOCOLPGM("Leveled:");
float leveled[XYZ] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] };
planner.apply_leveling(leveled);
report_xyz(leveled);
SERIAL_PROTOCOLPGM("UnLevel:");
float unleveled[XYZ] = { leveled[X_AXIS], leveled[Y_AXIS], leveled[Z_AXIS] };
planner.unapply_leveling(unleveled);
report_xyz(unleveled);
#if IS_KINEMATIC
#if IS_SCARA
SERIAL_PROTOCOLPGM("ScaraK: ");
#else
SERIAL_PROTOCOLPGM("DeltaK: ");
#endif
inverse_kinematics(leveled); // writes delta[]
report_xyz(delta);
#endif
SERIAL_PROTOCOLPGM("Stepper:");
const float step_count[XYZE] = { stepper.position(X_AXIS), stepper.position(Y_AXIS), stepper.position(Z_AXIS), stepper.position(E_AXIS) };
report_xyze(step_count, 4, 0);
#if IS_SCARA
const float deg[XYZ] = {
stepper.get_axis_position_degrees(A_AXIS),
stepper.get_axis_position_degrees(B_AXIS)
};
SERIAL_PROTOCOLPGM("Degrees:");
report_xyze(deg, 2);
#endif
SERIAL_PROTOCOLPGM("FromStp:");
get_cartesian_from_steppers(); // writes cartes[XYZ] (with forward kinematics)
const float from_steppers[XYZE] = { cartes[X_AXIS], cartes[Y_AXIS], cartes[Z_AXIS], stepper.get_axis_position_mm(E_AXIS) };
report_xyze(from_steppers);
const float diff[XYZE] = {
from_steppers[X_AXIS] - leveled[X_AXIS],
from_steppers[Y_AXIS] - leveled[Y_AXIS],
from_steppers[Z_AXIS] - leveled[Z_AXIS],
from_steppers[E_AXIS] - current_position[E_AXIS]
};
SERIAL_PROTOCOLPGM("Differ: ");
report_xyze(diff);
}
struct command gen_velocity_cmd(int8_t v_x, int8_t v_y, int8_t w, bool control_ang_vels)
{
struct vel_profile vels = inverse_kinematics(v_x, v_y, w);
struct command cmd;
control_ang_vels ? (cmd.cmd[0] = 7) : (cmd.cmd[0] = 3);
cmd.cmd[1] = vels.for_left;
cmd.cmd[2] = vels.for_right;
cmd.cmd[3] = vels.rear_right;
cmd.cmd[4] = vels.rear_left;
cmd.cmd_size = 5;
sprintf(cmd.debug_msg, "[atherosd] {%d} M1: %4d, M2: %4d, M3: %4d, M4: %4d, Control Angular Velocity: %s\n", cmd.cmd[0],
cmd.cmd[1], cmd.cmd[2], cmd.cmd[3], cmd.cmd[4], control_ang_vels ? "TRUE" : "FALSE");
return cmd;
}
int main(int argc, char *argv[])
{
int ret;
int choice;
int i, j;
float Ts=0.002;
float T;
float vm=0.05; //mean speed
matrix xd, xd_dot, J;
float q0[5], q_final[5];
float p0[3];
float p1[3];
DUNIT *unit;
unsigned short motor[NUM_MOTORS];
unsigned char sensor[NUM_SENSORS];
pSM = (SM*) mbuff_alloc(NAME_OF_MEMORY, sizeof(SM));
if(pSM == NULL) {
perror("mbuff_alloc failed");
return -1;
}
// Header = 1,;
// Step = 2,
motor[0] = 0;
motor[1] = 0;
motor[2] = 2100;
motor[3] = 4200;
motor[4] = -2500;
motor[5] = 0;
motor[6] = 18100;
motor[7] = -2100;
motor[8] = 1000;
motor[9] = 6200;
motor[10] = 0;
motor[11] = 0;
motor[12] = -2100;
motor[13] = -4180;
motor[14] = 2500;
motor[15] = 0;
motor[16] = -18810;
motor[17] = 2000;
motor[18] = -1000;
motor[19] = -6200;
motor[20] = 2100;
motor[21] = 60;
motor[22] = 60;
sensor[0] = 'R';
sensor[1] = 'R';
sensor[2] = 'R';
sensor[3] = 'R';
unit = (DUNIT*)&(pSM->Data.IntDat);
ret = send_commands(motor, sensor, unit);
pSM->VarIF.ResRep = 0;
pSM->VarIF.Mode = IDLE;
pSM->VarIF.InterruptSend = TRUE;
//watch return
readret(unit);
//desired final configuration
//q_final[0]=PI*3/5;
//q_final[1]=PI/3;
//q_final[2]=PI/6;
//q_final[3]=-PI/4;
//q_final[4]=0;
//evaluate_position(q_final, p1);
q0[4]=0;
open_hand (motor, sensor, unit);
while (1)
{
//present position
read_positions(unit, motor);
robot2DH (&motor[16], q0);
evaluate_position(q0, p0);
printf("\nPresent position: (%f,%f,%f)\n",p0[0],p0[1],p0[2]);
//select desired position
choice=select_desired_position (p1);
if (choice==-1) break;
if (choice==1) close_hand (motor, sensor, unit);
if (choice==2) open_hand (motor, sensor, unit);
printf("\nDesired final position: (%f,%f,%f)\n",p1[0],p1[1],p1[2]);
//execution time
T=evaluate_execution_time (p0, p1, vm);
//initializing matrixes
initialize_matrix (&xd, T/Ts+1, 3);
initialize_matrix (&xd_dot, T/Ts+1, 3);
initialize_matrix (&J, 3, 5);
//evaluate desired trajectory
trajectory_left_arm (Ts, T, p0, p1, &xd, &xd_dot);
//inverse kinematics
inverse_kinematics (Ts, T, &xd, &xd_dot, unit);
}
//close_hand (motor, sensor, unit);
free(xd.vector);
free(xd_dot.vector);
free(J.vector);
//free memory
mbuff_free(NAME_OF_MEMORY, (void*)pSM);
return 0;
}
