Commit 040a0ce6e6d0899add07d9605a92b54c38559583

Authored by Ricardo Rico Uribe
1 parent 9b68829e

corrections of arduino code

.vscode/settings.json 0 → 100644
@@ -0,0 +1,8 @@ @@ -0,0 +1,8 @@
  1 +{
  2 + "ros.distro": "melodic",
  3 + "python.autoComplete.extraPaths": [
  4 + "/home/ric/media/Aquanaute/aquanaute_ws/devel/lib/python2.7/dist-packages",
  5 + "/opt/ros/melodic/lib/python2.7/dist-packages"
  6 + ],
  7 + "cmake.configureOnOpen": false
  8 +}
0 \ No newline at end of file 9 \ No newline at end of file
arduino_code/adapter_motor/adapter_motor.ino
@@ -28,6 +28,23 @@ const byte COMM_FULL_FWD = 127; @@ -28,6 +28,23 @@ const byte COMM_FULL_FWD = 127;
28 const byte COMM_MED_FWD = 100; 28 const byte COMM_MED_FWD = 100;
29 const byte COMM_FULL_BCK = 1; 29 const byte COMM_FULL_BCK = 1;
30 const byte COMM_MED_BCK = 28; 30 const byte COMM_MED_BCK = 28;
  31 +
  32 +const byte RADIO_SCALE_CENTER = 50;
  33 +const byte RADIO_SCALE_LEFT = 0;
  34 +const byte RADIO_SCALE_RIGHT = 100;
  35 +
  36 +const byte MAX_ERROR_BEFORE_SPEED_IS_SET_TO_MAX = 30;
  37 +const byte ERROR_CENTER_THRESHOLD = 1;
  38 +const byte ERROR_MIN = -50;
  39 +const byte ERROR_MAX = 50;
  40 +//scale (-50,50) to (1-127) f(x) = (b-a)(x - min)/max - min + a
  41 +const float SPEED_COMMAND_SCALE_COEFFICIENT = (COMM_FULL_FWD-COMM_FULL_BCK)/(ERROR_MAX - ERROR_MIN)
  42 +
  43 +const byte TOTAL_OF_MESSAGES = 4;
  44 +const byte BYTE_CARRY_LEFT = 8;
  45 +
  46 +const byte HEX_CHAR_C =0x43; //Char "C", by design this is the message that the encoder expects to respond with its position
  47 +const word FIFTY_HZ = 331; // = (16*10^6) / (50*1024) - 1 (must be <65536) // 311 = 50Hz
31 //***** Variables *****// 48 //***** Variables *****//
32 byte speed_command = COMM_ALL_STOP; //Message to send to the Motor Controller 49 byte speed_command = COMM_ALL_STOP; //Message to send to the Motor Controller
33 50
@@ -64,7 +81,7 @@ void setup() @@ -64,7 +81,7 @@ void setup()
64 TCCR4B = 0; // same for TCCR4B 81 TCCR4B = 0; // same for TCCR4B
65 TCNT4 = 0; //initialize counter value to 0 82 TCNT4 = 0; //initialize counter value to 0
66 // set compare match register for 20hz increments 83 // set compare match register for 20hz increments
67 - OCR4A = 311 / 1; // = (16*10^6) / (50*1024) - 1 (must be <65536) // 311 = 50Hz / // turn on CTC mode 84 + OCR4A = FIFTY_HZ;// = (16*10^6) / (50*1024) - 1 (must be <65536) // 311 = 50Hz // turn on CTC mode
68 TCCR4B |= (1 << WGM12); 85 TCCR4B |= (1 << WGM12);
69 // Set CS12 and CS10 bits for 1024 prescaler 86 // Set CS12 and CS10 bits for 1024 prescaler
70 TCCR4B |= (1 << CS12) | (1 << CS10); 87 TCCR4B |= (1 << CS12) | (1 << CS10);
@@ -81,7 +98,7 @@ void setup() @@ -81,7 +98,7 @@ void setup()
81 /*Request the motor position, with a frecuency of 50Hz*/ 98 /*Request the motor position, with a frecuency of 50Hz*/
82 ISR(TIMER4_COMPA_vect) 99 ISR(TIMER4_COMPA_vect)
83 { 100 {
84 - Serial2.write(0x43); //Char "C", by design this is the message that the encoder expects to respond with its position 101 + Serial2.write(HEX_CHAR_C);
85 count = 0; //reset of message counter 102 count = 0; //reset of message counter
86 first = true; //reset of first message 103 first = true; //reset of first message
87 } 104 }
@@ -104,19 +121,19 @@ void pwm() @@ -104,19 +121,19 @@ void pwm()
104 return; 121 return;
105 if (delta_us <= RADIO_CENTER + RADIO_THRESHOLD && delta_us >= RADIO_CENTER - RADIO_THRESHOLD) 122 if (delta_us <= RADIO_CENTER + RADIO_THRESHOLD && delta_us >= RADIO_CENTER - RADIO_THRESHOLD)
106 { 123 {
107 - scale_radio = 50; 124 + scale_radio = RADIO_SCALE_CENTER;
108 } 125 }
109 else if (delta_us <= RADIO_LEFT + RADIO_THRESHOLD) 126 else if (delta_us <= RADIO_LEFT + RADIO_THRESHOLD)
110 { 127 {
111 - scale_radio = 0; 128 + scale_radio = RADIO_SCALE_LEFT;
112 } 129 }
113 else if (delta_us >= RADIO_RIGHT - RADIO_THRESHOLD) 130 else if (delta_us >= RADIO_RIGHT - RADIO_THRESHOLD)
114 { 131 {
115 - scale_radio = 100; 132 + scale_radio = RADIO_SCALE_RIGHT;
116 } 133 }
117 else 134 else
118 { 135 {
119 - scale_radio = (delta_us - RADIO_LEFT) * 100 / (RADIO_RANGE); 136 + scale_radio = (delta_us - RADIO_LEFT) * 100 / (RADIO_RANGE); //scale the recieved radio to a value between 0-100
120 } 137 }
121 go_print = true; 138 go_print = true;
122 } 139 }
@@ -141,11 +158,11 @@ void serialEvent2() @@ -141,11 +158,11 @@ void serialEvent2()
141 } 158 }
142 else //chain all the bytes to have the number that indicates a position 159 else //chain all the bytes to have the number that indicates a position
143 { 160 {
144 - recv = (recv << 8) + Serial2.read(); 161 + recv = (recv << BYTE_CARRY_LEFT) + Serial2.read();
145 } 162 }
146 count++; 163 count++;
147 } 164 }
148 - if (count == 4) //when all 4 bytes of the message have been recieved, do the control 165 + if (count == TOTAL_OF_MESSAGES) //when all 4 bytes of the message have been recieved, do the control
149 { 166 {
150 pos = recv; 167 pos = recv;
151 count = 0; 168 count = 0;
@@ -181,21 +198,21 @@ void serialEvent2() @@ -181,21 +198,21 @@ void serialEvent2()
181 198
182 int v = _kp * error_pos; //Control 199 int v = _kp * error_pos; //Control
183 200
184 - if (v >= -1 && v <= 1) //if the error is at 0 with a threshold, send stop to the motor 201 + if (v >= -ERROR_CENTER_THRESHOLD && v <= ERROR_CENTER_THRESHOLD) //if the error is at 0 with a threshold, send stop to the motor
185 { 202 {
186 speed_command = COMM_STOP; 203 speed_command = COMM_STOP;
187 } 204 }
188 - 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 205 + 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
189 { 206 {
190 speed_command = COMM_FULL_FWD; 207 speed_command = COMM_FULL_FWD;
191 } 208 }
192 - 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 209 + 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
193 { 210 {
194 speed_command = COMM_FULL_BCK; 211 speed_command = COMM_FULL_BCK;
195 } 212 }
196 else //Calculate the speed with this formula that transforms the error (-50,50) into the speed command (1,127) 213 else //Calculate the speed with this formula that transforms the error (-50,50) into the speed command (1,127)
197 { 214 {
198 - speed_command = (1.26) * (v - 50) + 127; 215 + speed_command = SPEED_COMMAND_SCALE_COEFFICIENT * (v - ERROR_MIN) + COMM_FULL_BCK;
199 } 216 }
200 } 217 }
201 go_print = true; 218 go_print = true;