Commit 040a0ce6 authored by Ricardo Rico Uribe's avatar Ricardo Rico Uribe
Browse files

corrections of arduino code

parent 9b68829e
{
"ros.distro": "melodic",
"python.autoComplete.extraPaths": [
"/home/ric/media/Aquanaute/aquanaute_ws/devel/lib/python2.7/dist-packages",
"/opt/ros/melodic/lib/python2.7/dist-packages"
],
"cmake.configureOnOpen": false
}
\ No newline at end of file
......@@ -28,6 +28,23 @@ const byte COMM_FULL_FWD = 127;
const byte COMM_MED_FWD = 100;
const byte COMM_FULL_BCK = 1;
const byte COMM_MED_BCK = 28;
const byte RADIO_SCALE_CENTER = 50;
const byte RADIO_SCALE_LEFT = 0;
const byte RADIO_SCALE_RIGHT = 100;
const byte MAX_ERROR_BEFORE_SPEED_IS_SET_TO_MAX = 30;
const byte ERROR_CENTER_THRESHOLD = 1;
const byte ERROR_MIN = -50;
const byte ERROR_MAX = 50;
//scale (-50,50) to (1-127) f(x) = (b-a)(x - min)/max - min + a
const float SPEED_COMMAND_SCALE_COEFFICIENT = (COMM_FULL_FWD-COMM_FULL_BCK)/(ERROR_MAX - ERROR_MIN)
const byte TOTAL_OF_MESSAGES = 4;
const byte BYTE_CARRY_LEFT = 8;
const byte HEX_CHAR_C =0x43; //Char "C", by design this is the message that the encoder expects to respond with its position
const word FIFTY_HZ = 331; // = (16*10^6) / (50*1024) - 1 (must be <65536) // 311 = 50Hz
//***** Variables *****//
byte speed_command = COMM_ALL_STOP; //Message to send to the Motor Controller
......@@ -64,7 +81,7 @@ void setup()
TCCR4B = 0; // same for TCCR4B
TCNT4 = 0; //initialize counter value to 0
// set compare match register for 20hz increments
OCR4A = 311 / 1; // = (16*10^6) / (50*1024) - 1 (must be <65536) // 311 = 50Hz / // turn on CTC mode
OCR4A = FIFTY_HZ;// = (16*10^6) / (50*1024) - 1 (must be <65536) // 311 = 50Hz // turn on CTC mode
TCCR4B |= (1 << WGM12);
// Set CS12 and CS10 bits for 1024 prescaler
TCCR4B |= (1 << CS12) | (1 << CS10);
......@@ -81,7 +98,7 @@ void setup()
/*Request the motor position, with a frecuency of 50Hz*/
ISR(TIMER4_COMPA_vect)
{
Serial2.write(0x43); //Char "C", by design this is the message that the encoder expects to respond with its position
Serial2.write(HEX_CHAR_C);
count = 0; //reset of message counter
first = true; //reset of first message
}
......@@ -104,19 +121,19 @@ void pwm()
return;
if (delta_us <= RADIO_CENTER + RADIO_THRESHOLD && delta_us >= RADIO_CENTER - RADIO_THRESHOLD)
{
scale_radio = 50;
scale_radio = RADIO_SCALE_CENTER;
}
else if (delta_us <= RADIO_LEFT + RADIO_THRESHOLD)
{
scale_radio = 0;
scale_radio = RADIO_SCALE_LEFT;
}
else if (delta_us >= RADIO_RIGHT - RADIO_THRESHOLD)
{
scale_radio = 100;
scale_radio = RADIO_SCALE_RIGHT;
}
else
{
scale_radio = (delta_us - RADIO_LEFT) * 100 / (RADIO_RANGE);
scale_radio = (delta_us - RADIO_LEFT) * 100 / (RADIO_RANGE); //scale the recieved radio to a value between 0-100
}
go_print = true;
}
......@@ -141,11 +158,11 @@ void serialEvent2()
}
else //chain all the bytes to have the number that indicates a position
{
recv = (recv << 8) + Serial2.read();
recv = (recv << BYTE_CARRY_LEFT) + Serial2.read();
}
count++;
}
if (count == 4) //when all 4 bytes of the message have been recieved, do the control
if (count == TOTAL_OF_MESSAGES) //when all 4 bytes of the message have been recieved, do the control
{
pos = recv;
count = 0;
......@@ -181,21 +198,21 @@ void serialEvent2()
int v = _kp * error_pos; //Control
if (v >= -1 && v <= 1) //if the error is at 0 with a threshold, send stop to the motor
if (v >= -ERROR_CENTER_THRESHOLD && v <= ERROR_CENTER_THRESHOLD) //if the error is at 0 with a threshold, send stop to the motor
{
speed_command = COMM_STOP;
}
else if (v >= 30) //if the error (-50,50) is more than "30" (value arbitrarily chosen) send the maximum speed, this is done to minimize the time of the mouvement
else if (v >= MAX_ERROR_BEFORE_SPEED_IS_SET_TO_MAX) //if the error (-50,50) is more than "30" (value arbitrarily chosen) send the maximum speed, this is done to minimize the time of the mouvement
{
speed_command = COMM_FULL_FWD;
}
else if (v <= -30) //if the error (-50,50) is less than "-30" (value arbitrarily chosen) send the maximum speed, this is done to minimize the time of the mouvement
else if (v <= -MAX_ERROR_BEFORE_SPEED_IS_SET_TO_MAX) //if the error (-50,50) is less than "-30" (value arbitrarily chosen) send the maximum speed, this is done to minimize the time of the mouvement
{
speed_command = COMM_FULL_BCK;
}
else //Calculate the speed with this formula that transforms the error (-50,50) into the speed command (1,127)
{
speed_command = (1.26) * (v - 50) + 127;
speed_command = SPEED_COMMAND_SCALE_COEFFICIENT * (v - ERROR_MIN) + COMM_FULL_BCK;
}
}
go_print = true;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment