void Robot::move(Direction dir) {
turn(dir);
left_motor_forward();
right_motor_forward();
for (uint16_t i = 0; i < STEPS::CELL; i++) {
if (readings.is_wall_front_close) {
return;
}
if (readings.is_wall_left_close) {
step_left(STEPS::DELAY / 2);
} else if (readings.is_wall_right_close) {
step_right(STEPS::DELAY / 2);
}
step_motors(STEPS::DELAY);
}
}
void Robot::turn(Direction towards) {
uint16_t steps = STEPS::TURN;
if (towards == Directions::left(facing)) {
left_motor_backward();
right_motor_forward();
} else if (towards == Directions::right(facing)) {
left_motor_forward();
right_motor_backward();
} else if (towards == Directions::opposite(facing)) {
left_motor_forward();
right_motor_backward();
steps *= 2;
} else {
return; //don't need to turn because already facing the right way
}
for (uint16_t i = 0; i < steps; i++) {
step_motors(STEPS::DELAY);
}
facing = towards;
}
KINEMATIC_STATE line_generator(KINEMATIC_STATE current_state, KINEMATIC_STATE target_state, void (*step_motors)(bool, bool, bool, bool, bool, bool, bool, bool, int) ){
float x1 = current_state.x;
float y1 = current_state.y;
float z1 = current_state.z;
float x2 = target_state.x;
float y2 = target_state.y;
float z2 = target_state.z;
float feed_rate_1 = current_state.feed_rate;
float feed_rate_2 = target_state.feed_rate;
float e1 = current_state.extrude_length;
float e2 = target_state.extrude_length;
float z1_error = current_state.z1_error;
float z2_error = current_state.z2_error;
float z3_error = current_state.z3_error;
//motor control booleans
bool z1_dir = false;
bool z2_dir = false;
bool z3_dir = false;
bool e_dir =false;
bool z1_step = false;
bool z3_step = false;
bool z2_step = false;
bool e_step =false;
//calulation values
float x = 0.0f;
float y = 0.0f;
float z = 0.0f;
float e = current_state.extrude_error;
float zzz1 = 0.0f;
float zzz3 = 0.0f;
float zzz2 = 0.0f;
float prev_zzz1 = 0.0f;
float prev_zzz3 = 0.0f;
float prev_zzz2 = 0.0f;
float delta_x = x2 - x1;
float delta_y = y2 - y1;
float delta_z = z2 - z1;
float delta_e = e2 - e1;
float ratio = 0.0f;
float distance = sqrtf(powf(delta_x,2) + powf(delta_y,2) + powf(delta_z,2));
int num_steps = (int)((distance / LINE_STEP_SIZE) + 0.5f); //add 0.5 so the number is rounded rather than truncated when converted to int
if (feed_rate_1 == 0.0f){
feed_rate_1 = 9000.0f;
}
int current_delay = (int)((float)Z_MM_STEP_SIZE / ((float)feed_rate_1/(float)FEED_RATE_CONSTANT));
int target_delay = (int)((float)Z_MM_STEP_SIZE / ((float)feed_rate_2/(float)FEED_RATE_CONSTANT));
float delay = (float)current_delay;
float delay_delta = (float)target_delay - (float)current_delay;
float delay_step = 0;
//int current_delay = 400;
//set the previous value to the start point
xyz_to_zzz(x1, y1, z1, &prev_zzz1, &prev_zzz2, &prev_zzz3);
float num_extrude_steps = (delta_e/EXTRUDE_PER_STEP);
float extrude_steps_per_itter =0;
if (num_steps > 0){
extrude_steps_per_itter = num_extrude_steps / (float)num_steps;
delay_step = delay_delta/(float)num_steps;
}else{
extrude_steps_per_itter = 0;
}
char debug_buffer[128];
sprintf(debug_buffer,"\tDELAY: %f\n", delay);
write_serial(debug_buffer);
sprintf(debug_buffer,"\tDELAY DELTA: %f\n", delay_delta);
write_serial(debug_buffer);
sprintf(debug_buffer,"\tDELAY STEP: %f\n", delay_step);
write_serial(debug_buffer);
int step = 0;
for(step = 0; step < num_steps; step++){
delay += delay_step;
ratio = ((float)step + 1) / ((float)num_steps);
x = x1 + ratio * delta_x;
y = y1 + ratio * delta_y;
z = z1 + ratio * delta_z;
xyz_to_zzz(x, y, z, &zzz1, &zzz2, &zzz3);
z1_error += prev_zzz1 - zzz1;
z2_error += prev_zzz2 - zzz2;
z3_error += prev_zzz3 - zzz3;
e += extrude_steps_per_itter;
if (e > 1.0f){
e_dir = true;
e_step = true;
e = e - 1.0f;
}else if(e < -1.0f){
e_dir = false;
e_step = true;
e = e + 1.0f;
}else{
e_step = false;
}
if(z1_error >= Z_MM_STEP_SIZE){
//step forward
z1_error = z1_error - Z_MM_STEP_SIZE;
z1_dir = false;
z1_step = true;
}
else if(z1_error <= -Z_MM_STEP_SIZE){
//step backward
z1_error = z1_error + Z_MM_STEP_SIZE;
z1_dir = true;
z1_step = true;
}
else{
z1_step = false;
}
if(z2_error >= Z_MM_STEP_SIZE){
//step forward
z2_error = z2_error - Z_MM_STEP_SIZE;
z2_dir = false;
z2_step = true;
}
else if(z2_error <= -Z_MM_STEP_SIZE){
//step backward
z2_error = z2_error + Z_MM_STEP_SIZE;
z2_dir = true;
z2_step = true;
}
else{
z2_step = false;
}
if(z3_error >= Z_MM_STEP_SIZE){
//step forward
z3_error = z3_error - Z_MM_STEP_SIZE;
z3_dir = false;
z3_step = true;
}
else if(z3_error <= -Z_MM_STEP_SIZE){
//step backward
z3_error = z3_error + Z_MM_STEP_SIZE;
z3_dir = true;
z3_step = true;
}
else{
z3_step = false;
}
step_motors(z1_dir, z1_step, z2_dir, z2_step, z3_dir, z3_step, e_dir, e_step, (int)delay);
prev_zzz1 = zzz1;
prev_zzz2 = zzz2;
prev_zzz3 = zzz3;
z1_step = false;
z2_step = false;
z3_step = false;
}
target_state.z1_error = z1_error;
target_state.z2_error = z2_error;
target_state.z3_error = z3_error;
//if e gets broken, clear it otherwise extrusion will be messed up on next run
if (e > 1.0f) e =0.0f;
if (e < 1.0f) e =0.0f;
target_state.extrude_error = e;
return target_state;
}
