protesis_03/servo-control.cpp

/*
 * servo-control.cpp
 *
 *  Created on: 12.01.2021
 *      Author: frank
 */

#include "servo-control.hpp"

int angle_to_dxl_servo_position(const int deg);
int rpm_to_dxl_servo_speed(const int rpm);

dxl_servo servos[SERVO_COUNT];

void servos_set_current_initial_position() {
	for(int i = 0; i < SERVO_COUNT ; i++){
		servos[i].intial_position = dxlGetPosition(servos[i].id);
	}
}

void servos_setup() {
	servos[0].id = 1;
	servos[0].intial_position = DXL_NOSE_MOTOR_INITIAL_POSITION;
	servos[0].initial_speed = DXL_NOSE_MOTOR_INITIAL_SPEED;
	servos[0].max_position = DXL_NOSE_MOTOR_MAX_POSITION_CCW;
	servos[0].min_position = DXL_NOSE_MOTOR_MIN_POSITION_CW;
	servos[0].torque_limit = DXL_NOSE_MOTOR_TORQUE_LIMIT;
	servos[0].joint_orientation = - 1;

	servos[1].id = 2;
	servos[1].intial_position = DXL_MID_MOTOR_INITIAL_POSITION;
	servos[1].initial_speed = DXL_MID_MOTOR_INITIAL_SPEED;
	servos[1].max_position = DXL_MID_MOTOR_MAX_POSITION_CCW;
	servos[1].min_position = DXL_MID_MOTOR_MIN_POSITION_CW;
	servos[1].torque_limit = DXL_MID_MOTOR_TORQUE_LIMIT;
	servos[1].joint_orientation = - 1;

	servos[2].id = 3;
	servos[2].intial_position = DXL_MID_MOTOR_INITIAL_POSITION;
	servos[2].initial_speed = DXL_MID_MOTOR_INITIAL_SPEED;
	servos[2].max_position = DXL_MID_MOTOR_MAX_POSITION_CCW;
	servos[2].min_position = DXL_MID_MOTOR_MIN_POSITION_CW;
	servos[2].torque_limit = DXL_MID_MOTOR_TORQUE_LIMIT;
	servos[2].joint_orientation = + 1;

	servos[3].id = 4;
	servos[3].intial_position = DXL_MID_MOTOR_INITIAL_POSITION;
	servos[3].initial_speed = DXL_MID_MOTOR_INITIAL_SPEED;
	servos[3].max_position = DXL_MID_MOTOR_MAX_POSITION_CCW;
	servos[3].min_position = DXL_MID_MOTOR_MIN_POSITION_CW;
	servos[3].torque_limit = DXL_MID_MOTOR_TORQUE_LIMIT;
	servos[3].joint_orientation = + 1;

	servos[4].id = 5;
	servos[4].intial_position = DXL_BASE_MOTOR_INITIAL_POSITION;
	servos[4].initial_speed = DXL_BASE_MOTOR_INITIAL_SPEED;
	servos[4].max_position = DXL_BASE_MOTOR_MAX_POSITION_CCW;
	servos[4].min_position = DXL_BASE_MOTOR_MIN_POSITION_CW;
	servos[4].torque_limit = DXL_BASE_MOTOR_TORQUE_LIMIT;
	servos[4].joint_orientation = - 1;
}

void servos_init_with_fixed_position() {
	servos_setup();
	servos_init();
}

void servos_init_with_current_position() {
	servos_setup();
	servos_set_current_initial_position();
	servos_init();
}

void servos_init() {
	for(int i = 0; i< SERVO_COUNT; i++) {
    dxlSetStatusReturnLevel(servos[i].id, AX_RETURN_READ); // Incorporated to avoid the colapsing of data found it with the osciloscope when the motors are sending data to te board
		dxlSetJointMode(servos[i].id, servos[i].min_position, servos[i].max_position);
		dxlSetGoalSpeed(servos[i].id, servos[i].initial_speed);
		dxlTorqueOn(servos[i].id);
		dxlSetRunningTorqueLimit(servos[i].id,MOTOR_TORQUE_LIMIT / MOTOR_TORQUE_RATIO);
		dxlSetGoalPosition(servos[i].id, servos[i].intial_position);
	}
}

void servo_init(const int servo_id) {
	int index = servo_id - 1;
	dxlSetJointMode(servos[index].id, servos[index].min_position, servos[index].max_position);
	dxlSetGoalSpeed(servos[index].id, servos[index].initial_speed);
	dxlSetGoalPosition(servos[index].id, servos[index].intial_position);
}

void servo_to_initial_position(const int servo_id) {
	dxlSetGoalPosition(servo_id, DXL_MID_MOTOR_INITIAL_POSITION);
}

void servos_to_initial_position() {
	for(int i = 0; i < SERVO_COUNT; i++) {
		dxlSetGoalPosition(servos[i].id, servos[i].intial_position);
	}
}

int servo_moving(const int servo_id) {
	return dxlGetMoving(servo_id);
}

int servos_moving() {
	int counter = 0;
	for(int i = 0; i < SERVO_COUNT; i++) {
		counter += dxlGetMoving(servos[i].id);
	}

	if(counter == SERVO_COUNT) {
		return 1;
	} else {
		return 0;
	}
}

void servos_in_position_wait() {
	while(servos_moving() == 1);
}

void servo_in_position_wait(const int servo_id) {
	while(dxlGetMoving(servo_id));
}


int servo_set_speed_rpm(const int servo_id, const int rpm) {
	dxlSetGoalSpeed(servo_id, rpm);
	return rpm;
}

void servo_move_to_angle(const int servo_id, const int deg) {
	dxlSetGoalPosition(servo_id, angle_to_dxl_servo_position(deg));
}

void map_angle_to_servo_with_initial_position(const int servo_id, const int deg) {
	dxlSetGoalPosition(servo_id, servos[servo_id -1].intial_position + servos[servo_id - 1].joint_orientation * angle_to_dxl_servo_position(deg));
}

void servos_move_to_angle(const int deg) {
	for(int i = 0; i < SERVO_COUNT; i++) {
		dxlSetGoalPosition(servos[i].id, angle_to_dxl_servo_position(deg));
	}
}

int angle_to_dxl_servo_position(const int deg) {
	return deg / ANGLE_PER_DIGIT_RATIO;
}

int rpm_to_dxl_servo_speed(const int rpm) {
	return rpm / RPM_TO_SPEED_RATIO;
}

void map_roll_to_servos_and_move(const int roll){
	dxlSetGoalPosition(servos[0].id, servos[0].joint_orientation * angle_to_dxl_servo_position(roll));
	dxlSetGoalPosition(servos[2].id, servos[2].joint_orientation * angle_to_dxl_servo_position(roll));
	dxlSetGoalPosition(servos[4].id, servos[4].joint_orientation * angle_to_dxl_servo_position(roll));
  delay(1);
}

void map_pitch_to_servos_and_move(const int pitch){
	dxlSetGoalPosition(servos[1].id, servos[1].joint_orientation * angle_to_dxl_servo_position(pitch));
	dxlSetGoalPosition(servos[3].id, servos[3].joint_orientation * angle_to_dxl_servo_position(pitch));
 delay(1);
}

void map_roll_to_servos_with_initial_position_and_move(const int roll){
	dxlSetGoalPosition(servos[0].id, servos[0].intial_position + (servos[0].joint_orientation * angle_to_dxl_servo_position(roll)));
	dxlSetGoalPosition(servos[2].id, servos[2].intial_position + (servos[2].joint_orientation * angle_to_dxl_servo_position(roll)));
	dxlSetGoalPosition(servos[4].id, servos[4].intial_position + (servos[4].joint_orientation * angle_to_dxl_servo_position(roll)));
  delay(1);
}

void map_pitch_to_servos_with_initial_position_and_move(const int pitch){
	dxlSetGoalPosition(servos[1].id, servos[1].intial_position + (servos[1].joint_orientation * angle_to_dxl_servo_position(pitch)));
	dxlSetGoalPosition(servos[3].id, servos[3].intial_position + (servos[3].joint_orientation * angle_to_dxl_servo_position(pitch)));
  delay(1);
}

int servo_calculate_torque_with_direction(const int servo_id) {
	int current_torque = dxlGetTorque(servo_id);
	int direction = current_torque & MOTOR_TORQUE_DIRECTION_MASK;
	current_torque &= MOTOR_TORQUE_VALUE_MASK;
	current_torque *= MOTOR_TORQUE_RATIO;
	if(direction == 0) {
		current_torque *= -1;
	}
	return current_torque;
}

void servos_collect_current_torque() {
	for(int i = 0; i < SERVO_COUNT; i++) {
		servos[i].current_torque = servo_calculate_torque_with_direction(servos[i].id);
	}
}

void servos_print_current_torque() {
	Serial.print("s_t=[");
	Serial.print(servos[SERVO_COUNT - 1].current_torque);
	for(int i=SERVO_COUNT - 2; i >= 0; i--) {
		Serial.print(",");
		Serial.print(servos[i].current_torque);
	}
	Serial.println("]");
}