From 87037b00945546a770722be719d74ea3707134b5 Mon Sep 17 00:00:00 2001 From: Dylan Thrush Date: Tue, 7 Aug 2018 00:28:11 -0700 Subject: [PATCH] Revert "Updated gripper firmware" This reverts commit 1c994875aa66690fc4b177714cc62dc596a47ee2. --- software/firmware/gripper/gripper.ino | 671 ++++++++------------------ 1 file changed, 190 insertions(+), 481 deletions(-) diff --git a/software/firmware/gripper/gripper.ino b/software/firmware/gripper/gripper.ino index 7ca78c5..0267115 100644 --- a/software/firmware/gripper/gripper.ino +++ b/software/firmware/gripper/gripper.ino @@ -1,306 +1,167 @@ ////////// Includes ////////// #include #include -#include ////////// Hardware / Data Enumerations ////////// enum HARDWARE { - RS485_EN = 14, - RS485_RX = 7, - RS485_TX = 8, + // RS485_EN = 2, + // RS485_RX = 7, + // RS485_TX = 8, - // Motor 1 - PINCH_MOTOR_PWM_1 = 25, - PINCH_MOTOR_PWM_2 = 22, - PINCH_ENCODER_A = 2, - PINCH_ENCODER_B = 30, - PINCH_MOTOR_CURRENT_SENSE = A2, + MOTOR_PWM_1 = 25, + MOTOR_PWM_2 = 22, + MOTOR_CURRENT_SENSE = A2, - // Motor 2 - FOREFINGER_MOTOR_PWM_1 = 23, - FOREFINGER_MOTOR_PWM_2 = 9, - FOREFINGER_ENCODER_A = 29, - FOREFINGER_ENCODER_B = 27, - FOREFINGER_MOTOR_CURRENT_SENSE = A3, + ENCODER_A = 2, + ENCODER_B = 30, - // Motor 3 - THUMB_MOTOR_PWM_1 = 10, - THUMB_MOTOR_PWM_2 = 20, - THUMB_ENCODER_A = 28, - THUMB_ENCODER_B = 12, - THUMB_MOTOR_CURRENT_SENSE = A5, - - // Motor 4 - MIDDLEFINGER_MOTOR_PWM_1 = 21, - MIDDLEFINGER_MOTOR_PWM_2 = 5, - MIDDLEFINGER_ENCODER_A = 11, - MIDDLEFINGER_ENCODER_B = 13, - MIDDLEFINGER_MOTOR_CURRENT_SENSE = A4, - - // LEDs LED_RED = 6, LED_GREEN = 32, LED_BLUE = 13, - WORKLIGHT = 15 -}; - -enum MG_INDEX { - PINCH = 0, - FOREFINGER = 1, - THUMB = 2, - MIDDLEFINGER = 3 -}; - -enum MODES { - NO_CHANGE = 0, - NORMAL = 1, - TWO_FINGER_PINCH = 2, - WIDE = 3, - SCISSOR = 4 -}; - -struct MOTOR_GROUP { - int PWM_1_PIN; - int PWM_2_PIN; - int CURRENT_PIN; - Encoder *ENC; - PID *PID_CONTROL; - double INPUT_POS; - double OUTPUT_POS; - double SETPOINT_POS; - int CURRENT_READING; - static const int NUM_READINGS = 20; - int READINGS[MOTOR_GROUP::NUM_READINGS]; - int READ_INDEX = 0; - int TOTAL = 0; - int TRIP_THRESHOLD; - int HOME_BACKOFF; - bool IS_HOMING = false; - int GRIP_THRESHOLD; - int TRAVEL_MAX; - int TRAVEL_MIN; - int LAST_GOOD_POSITION; - bool LAST_GOOD_POSITION_SET = false; -}; - -struct MOTOR_GROUP motor_groups[] = { - {HARDWARE::PINCH_MOTOR_PWM_1, - HARDWARE::PINCH_MOTOR_PWM_2, - HARDWARE::PINCH_MOTOR_CURRENT_SENSE}, - - {HARDWARE::FOREFINGER_MOTOR_PWM_1, - HARDWARE::FOREFINGER_MOTOR_PWM_2, - HARDWARE::FOREFINGER_MOTOR_CURRENT_SENSE}, - - {HARDWARE::THUMB_MOTOR_PWM_1, - HARDWARE::THUMB_MOTOR_PWM_2, - HARDWARE::THUMB_MOTOR_CURRENT_SENSE}, - - {HARDWARE::MIDDLEFINGER_MOTOR_PWM_1, - HARDWARE::MIDDLEFINGER_MOTOR_PWM_2, - HARDWARE::MIDDLEFINGER_MOTOR_CURRENT_SENSE} -}; - -enum MODBUS_REGISTERS { - // Inputs - MODE = 0, // if this is zero don't apply position updates, don't make changes - FINGER_POSITION = 1, - HOME = 2, - LIGHT_STATE = 3, - - // Outputs - CURRENT_PINCH = 4, - CURRENT_FOREFINGER = 5, - CURRENT_THUMB = 6, - CURRENT_MIDDLEFINGER = 7, - POSITION_PINCH = 8, - POSITION_FOREFINGER = 9, - POSITION_THUMB = 10, - POSITION_MIDDLEFINGER = 11, - LIGHT_STATE_OUPUT = 12, - MODE_OUTPUT = 13, - FINGER_POSITION_OUTPUT = 14, }; ////////// Global Variables ////////// -// Modbus stuff -const uint8_t node_id = 1; -const uint8_t mobus_serial_port_number = 3; -uint16_t modbus_data[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; -uint8_t num_modbus_registers = 0; -int8_t poll_state = 0; -bool communication_good = false; -uint8_t message_count = 0; +// define nice words for motor states +#define BRAKE 0 +#define FWD 1 +#define REV 2 +#define COAST 3 -// Define nice words for motor states -#define BRAKE 0 -#define FWD 1 -#define REV 2 -#define COAST 3 - -// PID gains -double kp = 5; -double ki = 0; -double kd = 0; - -// Timing -long prev_millis = 0; -unsigned int interval = 10; - -// Current offset -int current_midpoint = 375; - -// Joystick -int joy_value = 0; -int mid_value = 290; -int deadzone = 10; -int scale = 4; -long joy_interval = 10; - -// Limits -int universal_max = 6750; -int universal_min = -3000; - -int two_finger_pinch_travel_min = 0; -int two_finger_pinch_travel_max = 3000; - -int two_finger_pinch_setpoint = 3000; -int normal_setpoint = 3000; -int wide_setpoint = 2000; -int scissor_setpoint = 0; - -// Misc -int universal_position = 0; -int backoff = 50; -long curr_millis = 0; - -// Local modbus variables -bool global_home = false; -uint16_t global_state = 0; -uint16_t modbus_position = 0; -bool modbus_light_state = false; +long oldPosition = -999; +float velocity = 0; +long count = 0; +long oldposition = 0; +long newposition; +int interval = 10; +long prev_time = 0; +float conversion_factor = 1500/31.25; //60.0*1000.0/cpr; ////////// Class Instantiations ////////// // Encoder -Encoder encoder_pinch(HARDWARE::PINCH_ENCODER_A, HARDWARE::PINCH_ENCODER_B); -Encoder encoder_forefinger(HARDWARE::FOREFINGER_ENCODER_A, HARDWARE::FOREFINGER_ENCODER_B); -Encoder encoder_thumb(HARDWARE::THUMB_ENCODER_A, HARDWARE::THUMB_ENCODER_B); -Encoder encoder_middlefinger(HARDWARE::MIDDLEFINGER_ENCODER_A, HARDWARE::MIDDLEFINGER_ENCODER_B); +Encoder enc(HARDWARE::ENCODER_A, HARDWARE::ENCODER_B); -// PID -PID motor_pid_pinch(&motor_groups[MG_INDEX::PINCH].INPUT_POS, &motor_groups[MG_INDEX::PINCH].OUTPUT_POS, &motor_groups[MG_INDEX::PINCH].SETPOINT_POS, kp, ki, kd, DIRECT); -PID motor_pid_forefinger(&motor_groups[MG_INDEX::FOREFINGER].INPUT_POS, &motor_groups[MG_INDEX::FOREFINGER].OUTPUT_POS, &motor_groups[MG_INDEX::FOREFINGER].SETPOINT_POS, kp, ki, kd, DIRECT); -PID motor_pid_thumb(&motor_groups[MG_INDEX::THUMB].INPUT_POS, &motor_groups[MG_INDEX::THUMB].OUTPUT_POS, &motor_groups[MG_INDEX::THUMB].SETPOINT_POS, kp, ki, kd, DIRECT); -PID motor_pid_middlefinger(&motor_groups[MG_INDEX::MIDDLEFINGER].INPUT_POS, &motor_groups[MG_INDEX::MIDDLEFINGER].OUTPUT_POS, &motor_groups[MG_INDEX::MIDDLEFINGER].SETPOINT_POS, kp, ki, kd, DIRECT); +// Velocity +double setpoint_vel, input_vel, output_vel; //PID velocity variables +PID motor_vel(&input_vel, &output_vel, &setpoint_vel, 10, 1, 0, DIRECT); //velocity PID +double signed_setpoint_vel = -200; -// Modbus -Modbus slave(node_id, mobus_serial_port_number, HARDWARE::RS485_EN); +// Position +double setpoint_pos = 0; //PID position variables +double input_pos, output_pos; +PID motor_pos(&input_pos, &output_pos, &setpoint_pos, 500, 0, 0, DIRECT); //position PID + +long current_pos = 0; + +// Homing +int trip_threshold = 550; +bool home_flag = true; +long prev_millis = 0; ////////// Setup ////////// void setup() { setup_hardware(); - - num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]); - slave.begin(115200); - slave.setTimeOut(150); - Serial.begin(9600); + motor_vel.SetMode(AUTOMATIC); + motor_vel.SetSampleTime(1); + motor_pos.SetMode(AUTOMATIC); + motor_pos.SetSampleTime(1); - motor_groups[MG_INDEX::PINCH].ENC = &encoder_pinch; - motor_groups[MG_INDEX::FOREFINGER].ENC = &encoder_forefinger; - motor_groups[MG_INDEX::THUMB].ENC = &encoder_thumb; - motor_groups[MG_INDEX::MIDDLEFINGER].ENC = &encoder_middlefinger; - - motor_groups[MG_INDEX::PINCH].PID_CONTROL = &motor_pid_pinch; - motor_pid_pinch.SetMode(AUTOMATIC); - motor_pid_pinch.SetSampleTime(1); - - motor_groups[MG_INDEX::FOREFINGER].PID_CONTROL = &motor_pid_forefinger; - motor_pid_forefinger.SetMode(AUTOMATIC); - motor_pid_forefinger.SetSampleTime(1); - - motor_groups[MG_INDEX::THUMB].PID_CONTROL = &motor_pid_thumb; - motor_pid_thumb.SetMode(AUTOMATIC); - motor_pid_thumb.SetSampleTime(1); - - motor_groups[MG_INDEX::MIDDLEFINGER].PID_CONTROL = &motor_pid_middlefinger; - motor_pid_middlefinger.SetMode(AUTOMATIC); - motor_pid_middlefinger.SetSampleTime(1); - - // initialize all the readings to 0: - for (int i = 0; i < MOTOR_GROUP::NUM_READINGS; i++) { - motor_groups[0].READINGS[i] = 0; - motor_groups[1].READINGS[i] = 0; - motor_groups[2].READINGS[i] = 0; - motor_groups[3].READINGS[i] = 0; - } - - motor_groups[MG_INDEX::PINCH].TRIP_THRESHOLD = 200; - motor_groups[MG_INDEX::PINCH].HOME_BACKOFF = 200; - motor_groups[MG_INDEX::PINCH].GRIP_THRESHOLD = -100; - motor_groups[MG_INDEX::PINCH].TRAVEL_MAX = 6750; - motor_groups[MG_INDEX::PINCH].TRAVEL_MIN = 0; - motor_groups[MG_INDEX::PINCH].LAST_GOOD_POSITION = 0; - - motor_groups[MG_INDEX::FOREFINGER].TRIP_THRESHOLD = 200; - motor_groups[MG_INDEX::FOREFINGER].HOME_BACKOFF = 200; - motor_groups[MG_INDEX::FOREFINGER].GRIP_THRESHOLD = -100; - motor_groups[MG_INDEX::FOREFINGER].TRAVEL_MAX = 6750; - motor_groups[MG_INDEX::FOREFINGER].TRAVEL_MIN = 0; - motor_groups[MG_INDEX::FOREFINGER].LAST_GOOD_POSITION = 0; - - motor_groups[MG_INDEX::THUMB].TRIP_THRESHOLD = 200; - motor_groups[MG_INDEX::THUMB].HOME_BACKOFF = 200; - motor_groups[MG_INDEX::THUMB].GRIP_THRESHOLD = -100; - motor_groups[MG_INDEX::THUMB].TRAVEL_MAX = 6750; - motor_groups[MG_INDEX::THUMB].TRAVEL_MIN = 0; - motor_groups[MG_INDEX::THUMB].LAST_GOOD_POSITION = 0; - - motor_groups[MG_INDEX::MIDDLEFINGER].TRIP_THRESHOLD = 200; - motor_groups[MG_INDEX::MIDDLEFINGER].HOME_BACKOFF = 200; - motor_groups[MG_INDEX::MIDDLEFINGER].GRIP_THRESHOLD = -100; - motor_groups[MG_INDEX::MIDDLEFINGER].TRAVEL_MAX = 6750; - motor_groups[MG_INDEX::MIDDLEFINGER].TRAVEL_MIN = 0; - motor_groups[MG_INDEX::MIDDLEFINGER].LAST_GOOD_POSITION = 0; - + Serial.println("init"); } ////////// Loop ////////// void loop() { - poll_modbus(); - poll_sensors(); - set_leds(); - home_routine(); - set_motor_states(); + + // Position based homing + //Serial.println(analogRead(HARDWARE::MOTOR_CURRENT_SENSE)); + // if (home_flag == true){ + // Serial.println("homing"); + // + // // if it is time to move the motor and we haven't tripped the current yet: + // unsigned long curr_millis = millis(); + // if (curr_millis - prev_millis > interval){ + // prev_millis = curr_millis; + // current_pos -= 10; // increment by counts + // } + // if ((analogRead(HARDWARE::MOTOR_CURRENT_SENSE) > trip_threshold) && (curr_millis > 1000)){ + // enc.write(0); + // current_pos = 0; + // home_flag = false; + // Serial.println("homing complete"); + // } + // } + // + // set_position(current_pos); + // Serial.print("Set Pos: "); + // Serial.println(current_pos); + // Serial.print("Curr Pos: "); + // Serial.println(enc.read()); + // delay(1); + +/*----------------------------------------------------------------------------*/ + + // Position movement test + // unsigned long curr_millis = millis(); + // // if it is time to move the motor and we haven't tripped the current yet: + // if (curr_millis - prev_millis > interval){ + // prev_millis = curr_millis; + + // current_pos += 10; // increment by counts + + // } + // + set_position(current_pos); + + Serial.print("Set Pos: "); + Serial.println(current_pos); + Serial.print("Curr Pos: "); + Serial.println(enc.read()); + delay(1); + +/*----------------------------------------------------------------------------*/ + + // Velocity based non blocking homing + // if (home_flag == true){ + // + // set_velocity(-150); + // + // Serial.println("Homing"); + // + // Serial.print("Current: "); + // Serial.println(analogRead(HARDWARE::MOTOR_CURRENT_SENSE)); + // + // if ((analogRead(HARDWARE::MOTOR_CURRENT_SENSE) > trip_threshold) && (millis() > 500)){ + // Serial.println("Tripped current sensor!"); + // set_motor_output(BRAKE, 0); //stop moving + // home_flag = false; // no more homing + // //set_motor_output(COAST, 0); //stop moving + // enc.write(0); // reset encoder + // } + // } + // else { + // set_position(current_pos); // hold + // Serial.print("Set Pos: "); + // Serial.println(current_pos); + // Serial.print("Curr Pos: "); + // Serial.println(enc.read()); + // } + // + // delay(1); + } void setup_hardware(){ // setup IO - pinMode(HARDWARE::PINCH_MOTOR_PWM_1, OUTPUT); - pinMode(HARDWARE::PINCH_MOTOR_PWM_2, OUTPUT); - pinMode(HARDWARE::PINCH_MOTOR_CURRENT_SENSE, INPUT); - - pinMode(HARDWARE::FOREFINGER_MOTOR_PWM_1, OUTPUT); - pinMode(HARDWARE::FOREFINGER_MOTOR_PWM_2, OUTPUT); - pinMode(HARDWARE::FOREFINGER_MOTOR_CURRENT_SENSE, INPUT); - - pinMode(HARDWARE::THUMB_MOTOR_PWM_1, OUTPUT); - pinMode(HARDWARE::THUMB_MOTOR_PWM_2, OUTPUT); - pinMode(HARDWARE::THUMB_MOTOR_CURRENT_SENSE, INPUT); - - pinMode(HARDWARE::MIDDLEFINGER_MOTOR_PWM_1, OUTPUT); - pinMode(HARDWARE::MIDDLEFINGER_MOTOR_PWM_2, OUTPUT); - pinMode(HARDWARE::MIDDLEFINGER_MOTOR_CURRENT_SENSE, INPUT); + pinMode(HARDWARE::MOTOR_PWM_1, OUTPUT); + pinMode(HARDWARE::MOTOR_PWM_2, OUTPUT); + pinMode(HARDWARE::MOTOR_CURRENT_SENSE, INPUT); pinMode(HARDWARE::LED_RED, OUTPUT); pinMode(HARDWARE::LED_GREEN, OUTPUT); pinMode(HARDWARE::LED_BLUE, OUTPUT); // setup default states - set_motor_output(MG_INDEX::PINCH, BRAKE, 0); - set_motor_output(MG_INDEX::FOREFINGER, BRAKE, 0); - set_motor_output(MG_INDEX::THUMB, BRAKE, 0); - set_motor_output(MG_INDEX::MIDDLEFINGER, BRAKE, 0); + set_motor_output(COAST, 0); digitalWrite(HARDWARE::LED_RED, HIGH); digitalWrite(HARDWARE::LED_GREEN, HIGH); @@ -309,224 +170,72 @@ void setup_hardware(){ analogReadAveraging(32); } -void poll_modbus(){ - poll_state = slave.poll(modbus_data, num_modbus_registers); - communication_good = !slave.getTimeOutState(); +void home(int trip_threshold) { - // Set modes - if (modbus_data[MODBUS_REGISTERS::MODE] != MODES::NO_CHANGE){ - global_state = modbus_data[MODBUS_REGISTERS::MODE]; - modbus_position = modbus_data[MODBUS_REGISTERS::FINGER_POSITION]; - global_home = modbus_data[MODBUS_REGISTERS::HOME]; - modbus_light_state = modbus_data[MODBUS_REGISTERS::LIGHT_STATE]; + // change to position based homing + while (analogRead(HARDWARE::MOTOR_CURRENT_SENSE) > trip_threshold) { + current_pos = enc.read(); + set_position(current_pos + 10); + Serial.print("Current: "); + Serial.println(analogRead(HARDWARE::MOTOR_CURRENT_SENSE)); + } - // when shit dies - modbus_data[MODBUS_REGISTERS::LIGHT_STATE_OUPUT] = modbus_light_state; - modbus_data[MODBUS_REGISTERS::MODE_OUTPUT] = global_state; - modbus_data[MODBUS_REGISTERS::FINGER_POSITION_OUTPUT] = modbus_position; - } + set_motor_output(COAST, 0); //stop moving + enc.write(0); + Serial.println("Home complete"); } -void set_leds(){ - if(poll_state > 4){ - message_count++; - if(message_count > 2){ - digitalWrite(HARDWARE::LED_BLUE, !digitalRead(HARDWARE::LED_BLUE)); - message_count = 0; - } - digitalWrite(HARDWARE::LED_GREEN, LOW); - digitalWrite(HARDWARE::LED_RED, HIGH); - } - else if(!communication_good){ - digitalWrite(HARDWARE::LED_BLUE, LOW); - digitalWrite(HARDWARE::LED_GREEN, HIGH); - digitalWrite(HARDWARE::LED_RED, LOW); - } - digitalWrite(HARDWARE::WORKLIGHT, modbus_light_state); +void set_position(double setpoint_input){ + input_pos = enc.read(); //get actual current position + setpoint_pos = (double)setpoint_input; + //setpoint_pos = setpoint_input; + + //move to new position + if (input_pos > setpoint_pos) { //positive + motor_pos.SetControllerDirection(REVERSE); + motor_pos.Compute(); + set_motor_output(REV, output_pos); + } + else { //negative + motor_pos.SetControllerDirection(DIRECT); + motor_pos.Compute(); + set_motor_output(FWD, output_pos); + } } -void set_motor_states(){ - // update positions based on modes - if (!global_home){ - if (global_state == MODES::NORMAL){ - /* In normal operation the fingers are straight and parallel - universal_position moves the forefinger, thumb, and middlefinger while pinch is fixed. - */ - universal_position = map(modbus_position, 0, 65535, motor_groups[MG_INDEX::FOREFINGER].TRAVEL_MIN, motor_groups[MG_INDEX::FOREFINGER].TRAVEL_MAX); - - motor_groups[MG_INDEX::PINCH].SETPOINT_POS = normal_setpoint; - update_motor_position(MG_INDEX::FOREFINGER); - update_motor_position(MG_INDEX::THUMB); - update_motor_position(MG_INDEX::MIDDLEFINGER); - } - else if (global_state == MODES::TWO_FINGER_PINCH){ - /* In two finger pinch the pinch moves the fore and middle together - universal_position moves the forefinger, thumb, and middlefinger while pinch is fixed. - */ - universal_position = map(modbus_position, 0, 65535, two_finger_pinch_travel_min, two_finger_pinch_travel_max); - - motor_groups[MG_INDEX::PINCH].SETPOINT_POS = two_finger_pinch_setpoint; - update_motor_position(MG_INDEX::FOREFINGER); - update_motor_position(MG_INDEX::THUMB); - update_motor_position(MG_INDEX::MIDDLEFINGER); - } - else if (global_state == MODES::WIDE){ - /* In wide mode the pinch moves the fore and middle apart for a wider grip - universal_position moves the forefinger, thumb, and middlefinger while pinch is fixed. - */ - universal_position = map(modbus_position, 0, 65535, motor_groups[MG_INDEX::FOREFINGER].TRAVEL_MIN, motor_groups[MG_INDEX::FOREFINGER].TRAVEL_MAX); - - motor_groups[MG_INDEX::PINCH].SETPOINT_POS = wide_setpoint; - update_motor_position(MG_INDEX::FOREFINGER); - update_motor_position(MG_INDEX::THUMB); - update_motor_position(MG_INDEX::MIDDLEFINGER); - - } - else if (global_state == MODES::SCISSOR){ - /* In wide mode the pinch moves the fore and middle apart for a wider grip - universal_position moves the forefinger, thumb, and middlefinger while pinch is fixed. - */ - universal_position = map(modbus_position, 0, 65535, motor_groups[MG_INDEX::PINCH].TRAVEL_MIN, motor_groups[MG_INDEX::PINCH].TRAVEL_MAX); - - motor_groups[MG_INDEX::FOREFINGER].SETPOINT_POS = scissor_setpoint; - motor_groups[MG_INDEX::THUMB].SETPOINT_POS = scissor_setpoint; - motor_groups[MG_INDEX::MIDDLEFINGER].SETPOINT_POS = scissor_setpoint; - update_motor_position(MG_INDEX::PINCH); - } - } - // Update the position - set_position(MG_INDEX::PINCH); - set_position(MG_INDEX::FOREFINGER); - set_position(MG_INDEX::THUMB); - set_position(MG_INDEX::MIDDLEFINGER); +void set_velocity(double signed_setpoint_vel){ + // PID Velocity Control + if (signed_setpoint_vel > 0){ //positive + input_vel = calc_velocity(); + setpoint_vel = signed_setpoint_vel; + motor_vel.Compute(); + set_motor_output(FWD, output_vel); + } + else{ //negative + input_vel = abs(calc_velocity()); + setpoint_vel = abs(signed_setpoint_vel); + motor_vel.Compute(); + set_motor_output(REV, output_vel); + } + // Serial.print("Measured Velocity: "); + // Serial.println(input_vel); } -void home_routine(){ - if (global_home){ - // need to home all axis - // home 2 / 3 / 4 first - motor_groups[MG_INDEX::FOREFINGER].IS_HOMING = true; - motor_groups[MG_INDEX::THUMB].IS_HOMING = true; - motor_groups[MG_INDEX::MIDDLEFINGER].IS_HOMING = true; - // wait till done - if (!motor_groups[MG_INDEX::FOREFINGER].IS_HOMING && !motor_groups[MG_INDEX::THUMB].IS_HOMING && !motor_groups[MG_INDEX::MIDDLEFINGER].IS_HOMING){ - // home 1 - motor_groups[MG_INDEX::PINCH].IS_HOMING = true; - } - // reset - if (!motor_groups[MG_INDEX::FOREFINGER].IS_HOMING && !motor_groups[MG_INDEX::THUMB].IS_HOMING && !motor_groups[MG_INDEX::MIDDLEFINGER].IS_HOMING && !motor_groups[MG_INDEX::PINCH].IS_HOMING){ - global_home = false; - } - } - home(MG_INDEX::FOREFINGER); - home(MG_INDEX::THUMB); - home(MG_INDEX::MIDDLEFINGER); - home(MG_INDEX::PINCH); +float calc_velocity(){ + unsigned long current_time = millis(); + int delta_t = current_time - prev_time; + + if (delta_t > interval) { + prev_time = current_time; //reset time + newposition = enc.read(); //find the new position + count = newposition - oldposition; //find the count since the last interval + velocity = (float(count) / float(delta_t))*conversion_factor; //calculate velocity + oldposition = newposition; //set the old position + } + return velocity; } -void home(int motor){ - if (motor_groups[motor].IS_HOMING){ - // if it is time to move the motor and we haven't tripped the current yet: - unsigned long curr_millis = millis(); - if (curr_millis - prev_millis > interval){ - prev_millis = curr_millis; - motor_groups[motor].SETPOINT_POS -= 10; // increment by counts - } - if (motor_groups[motor].CURRENT_READING > motor_groups[motor].TRIP_THRESHOLD){ - motor_groups[motor].ENC->write(0); // - motor_groups[motor].SETPOINT_POS = motor_groups[motor].HOME_BACKOFF; // backoff - motor_groups[motor].IS_HOMING = false; // don't home anymore - } - } -} - -void poll_sensors(){ - - // Update current - get_current(MG_INDEX::PINCH); - get_current(MG_INDEX::FOREFINGER); - get_current(MG_INDEX::THUMB); - get_current(MG_INDEX::MIDDLEFINGER); - - // Update motor positions - motor_groups[MG_INDEX::PINCH].INPUT_POS = motor_groups[MG_INDEX::PINCH].ENC->read(); - motor_groups[MG_INDEX::FOREFINGER].INPUT_POS = motor_groups[MG_INDEX::FOREFINGER].ENC->read(); - motor_groups[MG_INDEX::THUMB].INPUT_POS = motor_groups[MG_INDEX::THUMB].ENC->read(); - motor_groups[MG_INDEX::MIDDLEFINGER].INPUT_POS = motor_groups[MG_INDEX::MIDDLEFINGER].ENC->read(); - - // Report current - modbus_data[MODBUS_REGISTERS::CURRENT_PINCH] = motor_groups[MG_INDEX::PINCH].CURRENT_READING; - modbus_data[MODBUS_REGISTERS::CURRENT_FOREFINGER] = motor_groups[MG_INDEX::FOREFINGER].CURRENT_READING; - modbus_data[MODBUS_REGISTERS::CURRENT_THUMB] = motor_groups[MG_INDEX::THUMB].CURRENT_READING; - modbus_data[MODBUS_REGISTERS::CURRENT_MIDDLEFINGER] = motor_groups[MG_INDEX::MIDDLEFINGER].CURRENT_READING; - - // Report motor position - modbus_data[MODBUS_REGISTERS::POSITION_PINCH] = motor_groups[MG_INDEX::PINCH].INPUT_POS; - modbus_data[MODBUS_REGISTERS::POSITION_FOREFINGER] = motor_groups[MG_INDEX::FOREFINGER].INPUT_POS; - modbus_data[MODBUS_REGISTERS::POSITION_THUMB] = motor_groups[MG_INDEX::THUMB].INPUT_POS; - modbus_data[MODBUS_REGISTERS::POSITION_MIDDLEFINGER] = motor_groups[MG_INDEX::MIDDLEFINGER].INPUT_POS; -} - -void update_motor_position(int motor){ - // If we overcurrent and the LGPS flag is not set, set the last good position and enable the flag - if (motor_groups[motor].CURRENT_READING < motor_groups[motor].GRIP_THRESHOLD){ - if(!motor_groups[motor].LAST_GOOD_POSITION_SET){ - motor_groups[motor].LAST_GOOD_POSITION = universal_position - backoff; - motor_groups[motor].LAST_GOOD_POSITION_SET = true; - } - } - // If we back off reset the LGPS flag - if (universal_position < motor_groups[motor].LAST_GOOD_POSITION){ - motor_groups[motor].LAST_GOOD_POSITION_SET = false; - } - // if the LGPS flag is set we don't want to move forward - if (motor_groups[motor].LAST_GOOD_POSITION_SET){ - motor_groups[motor].SETPOINT_POS = motor_groups[motor].LAST_GOOD_POSITION; - } - else{ - // set to universal position - motor_groups[motor].SETPOINT_POS = universal_position; - } - // Always constrain to the valid ranges - motor_groups[motor].SETPOINT_POS = constrain(motor_groups[motor].SETPOINT_POS, motor_groups[motor].TRAVEL_MIN, motor_groups[motor].TRAVEL_MAX); -} - -void get_current(int motor){ - // subtract the last reading: - motor_groups[motor].TOTAL = motor_groups[motor].TOTAL - motor_groups[motor].READINGS[motor_groups[motor].READ_INDEX]; - // read from the sensor: - motor_groups[motor].READINGS[motor_groups[motor].READ_INDEX] = analogRead(motor_groups[motor].CURRENT_PIN) - current_midpoint; - // add the reading to the TOTAL: - motor_groups[motor].TOTAL = motor_groups[motor].TOTAL + motor_groups[motor].READINGS[motor_groups[motor].READ_INDEX]; - // advance to the next position in the array: - motor_groups[motor].READ_INDEX++; - - // if we're at the end of the array... - if (motor_groups[motor].READ_INDEX >= motor_groups[motor].NUM_READINGS) { - // go to begginging: - motor_groups[motor].READ_INDEX = 0; - } - - // calculate the average: - motor_groups[motor].CURRENT_READING = motor_groups[motor].TOTAL / motor_groups[motor].NUM_READINGS; -} - -void set_position(int motor){ - - //move to new position - if (motor_groups[motor].INPUT_POS > motor_groups[motor].SETPOINT_POS) { //need to move negative - motor_groups[motor].PID_CONTROL->Compute(); - motor_groups[motor].PID_CONTROL->SetControllerDirection(REVERSE); - set_motor_output(motor, REV, motor_groups[motor].OUTPUT_POS); - } - else{ //need to move positive - motor_groups[motor].PID_CONTROL->SetControllerDirection(DIRECT); - motor_groups[motor].PID_CONTROL->Compute(); - set_motor_output(motor, FWD, motor_groups[motor].OUTPUT_POS); - } -} - -void set_motor_output(int motor, int direct, int pwm){ +void set_motor_output(uint8_t direct, uint8_t pwm){ /* Control Logic A | B @@ -537,29 +246,29 @@ void set_motor_output(int motor, int direct, int pwm){ */ // make sure the PWM doesn't go too high - // pwm = map(pwm, 0, 255, 0, 110); + pwm = map(pwm, 0, 255, 0, 110); if (direct <= 4){ switch (direct){ case 0: - analogWrite(motor_groups[motor].PWM_1_PIN, 255); - analogWrite(motor_groups[motor].PWM_2_PIN, 255); + digitalWrite(HARDWARE::MOTOR_PWM_1, HIGH); + digitalWrite(HARDWARE::MOTOR_PWM_2, HIGH); break; case 1: - analogWrite(motor_groups[motor].PWM_1_PIN, pwm); - analogWrite(motor_groups[motor].PWM_2_PIN, 0); + analogWrite(HARDWARE::MOTOR_PWM_1, pwm); + digitalWrite(HARDWARE::MOTOR_PWM_2, LOW); break; case 2: - analogWrite(motor_groups[motor].PWM_1_PIN, 0); - analogWrite(motor_groups[motor].PWM_2_PIN, pwm); + digitalWrite(HARDWARE::MOTOR_PWM_1, LOW); + analogWrite(HARDWARE::MOTOR_PWM_2, pwm); break; case 3: - analogWrite(motor_groups[motor].PWM_1_PIN, 0); - analogWrite(motor_groups[motor].PWM_2_PIN, 0); + digitalWrite(HARDWARE::MOTOR_PWM_1, LOW); + digitalWrite(HARDWARE::MOTOR_PWM_2, LOW); break; default: - analogWrite(motor_groups[motor].PWM_1_PIN, 0); - analogWrite(motor_groups[motor].PWM_1_PIN, 0); + digitalWrite(HARDWARE::MOTOR_PWM_1, LOW); + digitalWrite(HARDWARE::MOTOR_PWM_2, LOW); } } }