caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-08 10:11:14 +00:00
Code Issues Packages Projects Releases Wiki Activity

Revert "Updated gripper firmware"

Browse Source 
This reverts commit 1c994875aa.
This commit is contained in:
Dylan Thrush
2018-08-07 00:28:11 -07:00
parent 8d2e28ae16
commit 87037b0094
1 changed files with 190 additions and 481 deletions
Download Patch File Download Diff File Expand all files Collapse all files

663
software/firmware/gripper/gripper.ino
View File

@@ -1,306 +1,167 @@
////////// Includes //////////
#include <PID_v1.h>
#include <Encoder.h>
#include <ModbusRtu.h>
////////// 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 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];
// 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;
// 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));
}
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_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 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);
}
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){
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 (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);
if (input_pos > setpoint_pos) { //positive
motor_pos.SetControllerDirection(REVERSE);
motor_pos.Compute();
set_motor_output(REV, 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);
else { //negative
motor_pos.SetControllerDirection(DIRECT);
motor_pos.Compute();
set_motor_output(FWD, output_pos);
}
}
void set_motor_output(int motor, int direct, int pwm){
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);
}
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 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);
}
}
}