Commit 040a0ce6e6d0899add07d9605a92b54c38559583

Authored by Ricardo Rico Uribe
1 parent 9b68829e

corrections of arduino code

.vscode/settings.json 0 → 100644
... ... @@ -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 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 28 const byte COMM_MED_FWD = 100;
29 29 const byte COMM_FULL_BCK = 1;
30 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 48 //***** Variables *****//
32 49 byte speed_command = COMM_ALL_STOP; //Message to send to the Motor Controller
33 50  
... ... @@ -64,7 +81,7 @@ void setup()
64 81 TCCR4B = 0; // same for TCCR4B
65 82 TCNT4 = 0; //initialize counter value to 0
66 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 85 TCCR4B |= (1 << WGM12);
69 86 // Set CS12 and CS10 bits for 1024 prescaler
70 87 TCCR4B |= (1 << CS12) | (1 << CS10);
... ... @@ -81,7 +98,7 @@ void setup()
81 98 /*Request the motor position, with a frecuency of 50Hz*/
82 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 102 count = 0; //reset of message counter
86 103 first = true; //reset of first message
87 104 }
... ... @@ -104,19 +121,19 @@ void pwm()
104 121 return;
105 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 126 else if (delta_us <= RADIO_LEFT + RADIO_THRESHOLD)
110 127 {
111   - scale_radio = 0;
  128 + scale_radio = RADIO_SCALE_LEFT;
112 129 }
113 130 else if (delta_us >= RADIO_RIGHT - RADIO_THRESHOLD)
114 131 {
115   - scale_radio = 100;
  132 + scale_radio = RADIO_SCALE_RIGHT;
116 133 }
117 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 138 go_print = true;
122 139 }
... ... @@ -141,11 +158,11 @@ void serialEvent2()
141 158 }
142 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 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 167 pos = recv;
151 168 count = 0;
... ... @@ -181,21 +198,21 @@ void serialEvent2()
181 198  
182 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 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 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 211 speed_command = COMM_FULL_BCK;
195 212 }
196 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 218 go_print = true;
... ...