├── Images ├── sim1.jpg └── sim2.jpg ├── SMC3Utils v1.01.zip ├── README.md └── SMC3_v1_1 └── SMC3_v1_1.ino /Images/sim1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CraigHoffmann/race-motion-simulator/master/Images/sim1.jpg -------------------------------------------------------------------------------- /Images/sim2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CraigHoffmann/race-motion-simulator/master/Images/sim2.jpg -------------------------------------------------------------------------------- /SMC3Utils v1.01.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CraigHoffmann/race-motion-simulator/master/SMC3Utils v1.01.zip -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Motion Simulator for Online Motor Racing 2 | 3 | This is just a personal record of my motion simulator build and configuration for future reference. This type of simulator is commonly referred to as a seatmover and is ideal for driving simulation. The seat pivots and is driven by two motors to produce a roll and pitch action. These two seat movements can be used to simulate vehicle roll, pitch, acceleration, sway, and verticle heave (road surface noise). I also have a motorised seatbelt tensioner to assist in the braking simulation feel and another motor to provide traction loss motion which rotates the entire rig around a pivot point just in front of the foot pedals. 4 | 5 | ![SimSeat1](https://github.com/CraigHoffmann/race-motion-simulator/blob/master/Images/sim1.jpg?raw=true) 6 | 7 | ![SimSeat2](https://github.com/CraigHoffmann/race-motion-simulator/blob/master/Images/sim2.jpg?raw=true) 8 | 9 | Here are some videos of the rig in action... 10 | * An early version of the rig: https://www.youtube.com/watch?v=jcYm41Fejxo 11 | * Early build showing traction loss: https://www.youtube.com/watch?v=TVEkrrQ9q8A 12 | * A more recent video after rebuild: https://www.youtube.com/watch?v=rYA4waufOB0 13 | 14 | ## SMC3 Motor Driver 15 | SMC3 is the arduino uno based motor driver software I developed for the motion sim. 16 | 17 | ![image](https://user-images.githubusercontent.com/27387872/120056720-42da9b80-c07d-11eb-8954-a2bf82b0f45e.png) 18 | 19 | I released the code on xsimulator.net and it became one of the most popular drivers available due to it simplicity, flexibility and easy to source components. Here is a link to the discussion thread... https://www.xsimulator.net/community/threads/smc3-arduino-3dof-motor-driver-and-windows-utilities.4957/ 20 | 21 | ## SMC3 Windows Utilities 22 | To aid in the easy configuration of the motor driver and sim setup, I also developed a windows graphical app. 23 | 24 | ![image](https://user-images.githubusercontent.com/27387872/120056817-c5fbf180-c07d-11eb-8c67-f6a8f7997dda.png) 25 | 26 | Further details and discussion about the app can also be found on the xsimulator forum website link above. 27 | -------------------------------------------------------------------------------- /SMC3_v1_1/SMC3_v1_1.ino: -------------------------------------------------------------------------------- 1 | //**************************************************************************************************************** 2 | // SMC3.ini is basic Motor PID driver designed for motion simulators with upto 3 motors (written for UNO R3) 3 | // 4 | //**************************************************************************************************************** 5 | 6 | // Set to MODE1 for use with a typical H-Bride that requires PWM and 1 or 2 direction inputs 7 | // Set to MODE2 for a 43A "Chinese" IBT-2 H-Bridge from e-bay or equiv 8 | 9 | #define MODE2 10 | 11 | // Uncomment the following line to reverse the direction of Motor 1-3. 12 | 13 | #define REVERSE_MOTOR1 14 | 15 | // Uncomment ONE of the following lines to enable analogue input AN5 as a scaler for the motion values. 16 | 17 | // #define ENABLE_POT_SCALING 18 | #define ENABLE_NON_LINEAR_POT_SCALING 19 | 20 | // Uncomment the following lines to limit negative values for harness. 21 | 22 | //#define NO_NEGATIVE_MOTOR3 23 | #define NO_POSITIVE_MOTOR3 24 | 25 | // Uncomment the following line to enable the second software serial port. 26 | // NOTE: This is currently not working - leave commented out until fixed!!! 27 | 28 | // #define SECOND_SERIAL 29 | 30 | // COMMAND SET: 31 | // 32 | // [] Drive all motors to defined stop positions and hold there 33 | // [Axx],[Bxx],[Cxx] Send position updates for Motor 1,2,3 where xx is the binary position limitted to range 0-1024 34 | // [Dxx],[Exx],[Fxx] Send the Kp parameter for motor 1,2,3 where xx is the Kp value multiplied by 100 35 | // [Gxx],[Hxx],[Ixx] Send the Ki parameter for motor 1,2,3 where xx is the Ki value multiplied by 100 36 | // [Jxx],[Kxx],[Lxx] Send the Kd parameter for motor 1,2,3 where xx is the Kd value multiplied by 100 37 | // [Mxx],[Nxx],[Oxx] Send the Ks parameter for motor 1,2,3 where xx is the Ks value 38 | // [Pxy],[Qxy],[Rxy] Send the PWMmin and PWMmax values x is the PWMmin and y is PWMmax each being in range 0-255 39 | // [Sxy],[Txy],[Uxy] Send the Motor Min/Max Limits (x) and Input Min/Max Limits (y) (Note same value used for Min and Max) 40 | // [Vxy],[Wxy],[Xxy] Send the Feedback dead zone (x) and the PWM reverse duty (y) for each motor 41 | // [rdx] Read a value from the controller where x is the code for the parameter to read 42 | // [ena] Enable all motors 43 | // [sav] Save parameters to non-volatile memory 44 | // [ver] Send the SMC3 software version 45 | // 46 | 47 | // Arduino UNO Pinouts Used 48 | // 49 | // 9 - Motor 1 PWM 50 | // 10 - Motor 2 PWM 51 | // 11 - Motor 3 PWM 52 | // 2 - Motor 1 H-Bridge ENA 53 | // 3 - Motor 1 H-Bridge ENB 54 | // 4 - Motor 2 H-Bridge ENA 55 | // 5 - Motor 2 H-Bridge ENB 56 | // 6 - Motor 3 H-Bridge ENA 57 | // 7 - Motor 3 H-Bridge ENB 58 | // A0 - Motor 1 Feedback 59 | // A1 - Motor 2 Feedback 60 | // A2 - Motor 3 Feedback 61 | // A5 - Motion scaler pot input 62 | 63 | 64 | 65 | // BOF preprocessor bug prevention - leave me at the top of the arduino-code 66 | #if 1 67 | __asm volatile ("nop"); 68 | #endif 69 | 70 | #include 71 | #include 72 | 73 | // defines for setting and clearing register bits 74 | #ifndef cbi 75 | #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) 76 | #endif 77 | #ifndef sbi 78 | #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) 79 | #endif 80 | 81 | #define COM0 0 // hardware Serial Port 82 | #define COM1 1 // Software Serial Port 83 | #define START_BYTE '[' // Start Byte for serial commands 84 | #define END_BYTE ']' // End Byte for serial commands 85 | #define PROCESS_PERIOD_uS 250 // 244 -> 4096 per second approx 86 | #define TIMER_UPPER_LIMIT 4294966000 // Used to check for timer wraparound 87 | #define TIMER_LOWER_LIMIT 1000 // Used to check for timer wraparound 88 | 89 | 90 | unsigned long TimesUp=0; // Counter used to see if it's time to calculate next PID update 91 | 92 | int Feedback1 = 512; 93 | int Feedback2 = 512; 94 | int Feedback3 = 512; 95 | int Target1 = 512; 96 | int Target2 = 512; 97 | int Target3 = 512; 98 | int PotInput = 512; 99 | 100 | unsigned int RxByte[2]={0}; // Current byte received from each of the two comm ports 101 | int BufferEnd[2]={-1}; // Rx Buffer end index for each of the two comm ports 102 | unsigned int RxBuffer[5][2]={0}; // 5 byte Rx Command Buffer for each of the two comm ports 103 | unsigned long LoopCount = 0; // unsigned 32bit, 0 to 4,294,967,295 to count times round loop 104 | unsigned long LastCount = 0; // loop counter the last time it was read so can calc delta 105 | byte errorcount = 0; // serial receive error detected by invalid packet start/end bytes 106 | unsigned int CommsTimeout = 0; // used to reduce motor power if there has been no comms for a while 107 | byte PowerScale = 7; // used to divide(shift) PID result changes when in low power 108 | 109 | // 110 | // 111 | // +----+ +------+ 112 | // +-|PWR |--------------| USB |-----+ 113 | // | | | | | | 114 | // | +----+ +------+ o| 115 | // | o| 116 | // | o|AREF 117 | // | o|GND 118 | // NC|o o|13 TTL (software) Serial 119 | // IOREF|o +----+ o|12 TTL (software) Serial 120 | // RESET|o | | o|11~ PWM Motor 3 121 | // 3.3V|o | | o|10~ PWM Motor 2 122 | // 5V|o | | Arduino o|9~ PWM Motor 1 123 | // GND|o | | UNO o|8 124 | // GND|o | | | 125 | // VIN|o | | o|7 Motor 3 ENB 126 | // | | | o|6~ Motor 3 ENA 127 | // Motor 1 Pot A0|o | | o|5~ Motor 2 ENB 128 | // Motor 2 Pot A1|o | | o|4 Motor 2 ENA 129 | // Motor 3 Pot A2|o | | o|3~ Motor 1 ENB 130 | // A3|o | | o|2 Motor 1 ENA 131 | // A4|o | | o|1 132 | // Motion Scaler A5|o +-/\-+ o|0 133 | // +-\ /---------+ 134 | // \--------------------/ 135 | // 136 | // 137 | 138 | int OutputPort =PORTD; // read the current port D bit mask 139 | const int ENApin1 =2; // ENA output pin for Motor H-Bridge 1 (ie PortD bit position) 140 | const int ENBpin1 =3; // ENB output pin for Motor H-Bridge 1 (ie PortD bit position) 141 | const int ENApin2 =4; // ENA output pin for Motor H-Bridge 2 (ie PortD bit position) 142 | const int ENBpin2 =5; // ENB output pin for Motor H-Bridge 2 (ie PortD bit position) 143 | const int ENApin3 =6; // ENA output pin for Motor H-Bridge 3 (ie PortD bit position) 144 | const int ENBpin3 =7; // ENB output pin for Motor H-Bridge 3 (ie PortD bit position) 145 | const int PWMpin1 =9; // PWM output pin for Motor 1 146 | const int PWMpin2 =10; // PWM output pin for Motor 2 147 | const int PWMpin3 =11; // PWM output pin for Motor 3 148 | const int FeedbackPin1 = A0; // Motor 1 feedback pin 149 | const int FeedbackPin2 = A1; // Motor 2 feedback pin 150 | const int FeedbackPin3 = A2; // Motor 3 feedback pin 151 | const int PotInputPin = A5; // User adjustable POT used to scale the motion (if enabled) 152 | 153 | int DeadZone1 = 0; // feedback deadzone 154 | int DeadZone2 = 0; // feedback deadzone 155 | int DeadZone3 = 0; // feedback deadzone 156 | 157 | int CenterOffset1 = 0; // Adjust center offset of feedback position 158 | int CenterOffset2 = 0; // Adjust center offset of feedback position 159 | int CenterOffset3 = 0; // Adjust center offset of feedback position 160 | 161 | int CutoffLimitMax1 = 1000; // The position beyond which the motors are disabled 162 | int CutoffLimitMax2 = 1000; // The position beyond which the motors are disabled 163 | int CutoffLimitMax3 = 1000; // The position beyond which the motors are disabled 164 | int CutoffLimitMin1 = 23; // The position beyond which the motors are disabled 165 | int CutoffLimitMin2 = 23; // The position beyond which the motors are disabled 166 | int CutoffLimitMin3 = 23; // The position beyond which the motors are disabled 167 | 168 | int InputClipMax1 = 923; // The input position beyond which the target input is clipped 169 | int InputClipMax2 = 923; // The input position beyond which the target input is clipped 170 | int InputClipMax3 = 923; // The input position beyond which the target input is clipped 171 | int InputClipMin1 = 100; // The input position beyond which the target input is clipped 172 | int InputClipMin2 = 100; // The input position beyond which the target input is clipped 173 | int InputClipMin3 = 100; // The input position beyond which the target input is clipped 174 | 175 | 176 | int LiftFactor1 = 0; // was 10 - Increase PWM when driving motor in direction it has to work harder 177 | int LiftFactor2 = 0; // was 10 - Increase PWM when driving motor in direction it has to work harder 178 | 179 | int PIDProcessDivider = 1; // divider for the PID process timer 180 | int PIDProcessCounter = 0; 181 | int SerialFeedbackCounter = 0; 182 | int SerialFeedbackEnabled = 0; 183 | int SerialFeedbackPort = 0; 184 | 185 | int Ks1 = 1; 186 | long Kp1_x100 = 100; //initial value 187 | long Ki1_x100 = 40; 188 | long Kd1_x100 = 40; 189 | int Ks2 = 1; 190 | long Kp2_x100 = 420; 191 | long Ki2_x100 = 40; 192 | long Kd2_x100 = 40; 193 | int Ks3 = 1; 194 | int Kp3_x100 = 420; 195 | int Ki3_x100 = 40; 196 | int Kd3_x100 = 40; 197 | int PWMout1 = 0; 198 | int PWMout2 = 0; 199 | int PWMout3 = 0; 200 | 201 | int Disable1 = 1; //Motor stop flag 202 | int Disable2 = 1; //Motor stop flag 203 | int Disable3 = 1; //Motor stop flag 204 | int PWMoffset1 = 50; 205 | int PWMoffset2 = 50; 206 | int PWMoffset3 = 50; 207 | int PWMmax1 = 100; 208 | int PWMmax2 = 100; 209 | int PWMmax3 = 100; 210 | int PWMrev1 = 200; 211 | int PWMrev2 = 200; 212 | int PWMrev3 = 200; 213 | unsigned int Timer1FreqkHz = 25; // PWM freq used for Motor 1 and 2 214 | unsigned int Timer2FreqkHz = 31; // PWM freq used for Motor 3 215 | 216 | #ifdef SECOND_SERIAL 217 | SoftwareSerial mySerial(12, 13); // RX, TX 218 | #endif 219 | 220 | //**************************************************************************************************************** 221 | // Setup the PWM pin frequencies 222 | // 223 | //**************************************************************************************************************** 224 | 225 | 226 | void setPwmFrequency(int pin, int divisor) 227 | { 228 | byte mode; 229 | if(pin == 5 || pin == 6 || pin == 9 || pin == 10) 230 | { 231 | switch(divisor) 232 | { 233 | case 1: mode = 0x01; break; 234 | case 8: mode = 0x02; break; 235 | case 64: mode = 0x03; break; 236 | case 256: mode = 0x04; break; 237 | case 1024: mode = 0x05; break; 238 | default: return; 239 | } 240 | if(pin == 5 || pin == 6) 241 | { 242 | TCCR0B = TCCR0B & 0b11111000 | mode; 243 | } 244 | else 245 | { 246 | TCCR1B = TCCR1B & 0b11111000 | mode; 247 | } 248 | } 249 | else 250 | { 251 | if(pin == 3 || pin == 11) 252 | { 253 | switch(divisor) 254 | { 255 | case 1: mode = 0x01; break; 256 | case 8: mode = 0x02; break; 257 | case 32: mode = 0x03; break; 258 | case 64: mode = 0x04; break; 259 | case 128: mode = 0x05; break; 260 | case 256: mode = 0x06; break; 261 | case 1024: mode = 0x7; break; 262 | default: return; 263 | } 264 | TCCR2B = TCCR2B & 0b11111000 | mode; 265 | } 266 | } 267 | } 268 | 269 | void InitialisePWMTimer1(unsigned int Freq) // Used for pins 9 and 10 270 | { 271 | uint8_t wgm = 8; //setting the waveform generation mode to 8 - PWM Phase and Freq Correct 272 | TCCR1A = (TCCR1A & B11111100) | (wgm & B00000011); 273 | TCCR1B = (TCCR1B & B11100111) | ((wgm & B00001100) << 1); 274 | TCCR1B = (TCCR1B & B11111000) | 0x01; // Set the prescaler to minimum (ie divide by 1) 275 | 276 | unsigned int CountTOP; 277 | 278 | CountTOP = (F_CPU / 2) / Freq; // F_CPU is the oscillator frequency - Freq is the wanted PWM freq 279 | 280 | // Examples of CountTOP: 281 | // 400 = 20000Hz -> 8MHz / Freq = CountTOP 282 | // 320 = 25000Hz 283 | // 266 = 30075Hz 284 | 285 | ICR1 = CountTOP; // Set the TOP of the count for the PWM 286 | } 287 | 288 | 289 | void InitialisePWMTimer2(unsigned int Freq) 290 | { 291 | int Timer2DivideMode; 292 | 293 | if (Freq>=15000) 294 | { 295 | Timer2DivideMode=0x01; // Anything above 15000 set divide by 1 which gives 31250Hz PWM freq 296 | } 297 | else 298 | { 299 | Timer2DivideMode=0x02; // Anything below 15000 set divide by 8 (mode 2) which gives 3906Hz PWM freq 300 | } 301 | TCCR2B = TCCR2B & 0b11111000 | Timer2DivideMode; 302 | } 303 | 304 | 305 | void MyPWMWrite(uint8_t pin, uint8_t val) 306 | { 307 | #define OCR1A_MEM 0x88 308 | #define OCR1B_MEM 0x8A 309 | 310 | pinMode(pin, OUTPUT); 311 | 312 | //casting "val" to be larger so that the final value (which is the partially 313 | //the result of multiplying two potentially high value int16s) will not truncate 314 | uint32_t tmp = val; 315 | 316 | if (val == 0) 317 | digitalWrite(pin, LOW); 318 | else if (val == 255) 319 | digitalWrite(pin, HIGH); 320 | else 321 | { 322 | uint16_t regLoc16 = 0; 323 | uint16_t top; 324 | 325 | switch(pin) 326 | { 327 | case 9: 328 | sbi(TCCR1A, COM1A1); 329 | regLoc16 = OCR1A_MEM; 330 | top = ICR1; //Timer1_GetTop(); 331 | break; 332 | case 10: 333 | sbi(TCCR1A, COM1B1); 334 | regLoc16 = OCR1B_MEM; 335 | top = ICR1; //Timer1_GetTop(); 336 | break; 337 | } 338 | tmp=(tmp*top)/255; 339 | _SFR_MEM16(regLoc16) = tmp; //(tmp*top)/255; 340 | } 341 | } 342 | 343 | 344 | void DisableMotor1(); 345 | void DisableMotor2(); 346 | void DisableMotor3(); 347 | 348 | 349 | //**************************************************************************************************************** 350 | // Arduino setup subroutine called at startup/reset 351 | // 352 | //**************************************************************************************************************** 353 | 354 | 355 | void setup() 356 | { 357 | //Read all stored Parameters from EEPROM 358 | 359 | ReadEEProm(); 360 | 361 | Serial.begin(500000); //115200 362 | // set the data rate for the SoftwareSerial port 363 | #ifdef SECOND_SERIAL 364 | mySerial.begin(115200); 365 | #endif 366 | OutputPort=PORTD; 367 | pinMode(ENApin1, OUTPUT); 368 | pinMode(ENBpin1, OUTPUT); 369 | pinMode(ENApin2, OUTPUT); 370 | pinMode(ENBpin2, OUTPUT); 371 | pinMode(ENApin3, OUTPUT); 372 | pinMode(ENBpin3, OUTPUT); 373 | pinMode(PWMpin1, OUTPUT); 374 | pinMode(PWMpin2, OUTPUT); 375 | pinMode(PWMpin3, OUTPUT); 376 | MyPWMWrite(PWMpin1,0); //analogWrite(PWMpin1, 0); 377 | MyPWMWrite(PWMpin2,0); //analogWrite(PWMpin2, 0); 378 | analogWrite(PWMpin3, 0); 379 | DisableMotor1(); 380 | DisableMotor2(); 381 | DisableMotor3(); 382 | 383 | // Note that the base frequency for pins 3, 9, 10, and 11 is 31250 Hz 384 | //setPwmFrequency(PWMpin1, 1); // Divider can be 1, 8, 64, 256 385 | //setPwmFrequency(PWMpin2, 1); // Divider can be 1, 8, 64, 256 386 | //setPwmFrequency(PWMpin3, 1); // Divider can be 1, 8, 64, 256 387 | InitialisePWMTimer1(Timer1FreqkHz * 1000); // PWM Freq for Motor 1 and 2 can be set quite precisely - pass the desired freq and nearest selected 388 | InitialisePWMTimer2(Timer2FreqkHz * 1000); // Motor 3 PWM Divider can only be 1, 8, 64, 256 giving Freqs 31250,3906, 488 and 122 Hz 389 | 390 | //CLKPR = 0x80; // These two lines halve the processor clock 391 | //CLKPR = 0x01; 392 | 393 | // set analogue prescale to 16 394 | sbi(ADCSRA,ADPS2); 395 | cbi(ADCSRA,ADPS1); 396 | cbi(ADCSRA,ADPS0); 397 | } 398 | 399 | void WriteEEPRomWord(int address, int intvalue) 400 | { 401 | int low,high; 402 | high=intvalue/256; 403 | low=intvalue-(256*high); 404 | EEPROM.write(address,high); 405 | EEPROM.write(address+1,low); 406 | } 407 | 408 | int ReadEEPRomWord(int address) 409 | { 410 | int low,high, returnvalue; 411 | high=EEPROM.read(address); 412 | low=EEPROM.read(address+1); 413 | returnvalue=(high*256)+low; 414 | return returnvalue; 415 | } 416 | 417 | //**************************************************************************************************************** 418 | // Write all configurable parameters to EEPROM 419 | // 420 | //**************************************************************************************************************** 421 | 422 | 423 | void WriteEEProm() 424 | { 425 | EEPROM.write(0,114); 426 | EEPROM.write(1,CutoffLimitMin1); 427 | EEPROM.write(2,InputClipMin1); 428 | 429 | EEPROM.write(5,DeadZone1); 430 | EEPROM.write(6,CutoffLimitMin2); 431 | EEPROM.write(7,InputClipMin2); 432 | 433 | EEPROM.write(10,DeadZone2); 434 | WriteEEPRomWord(11,Kp1_x100); // **** Even though these variables are long the actual values stored are only 0 to 1000 so OK **** 435 | WriteEEPRomWord(13,Ki1_x100); 436 | WriteEEPRomWord(15,Kd1_x100); 437 | WriteEEPRomWord(17,Kp2_x100); 438 | WriteEEPRomWord(19,Ki2_x100); 439 | WriteEEPRomWord(21,Kd2_x100); 440 | EEPROM.write(23,PWMoffset1); 441 | EEPROM.write(24,PWMoffset2); 442 | EEPROM.write(25,PWMmax1); 443 | EEPROM.write(26,PWMmax2); 444 | EEPROM.write(27,PWMrev1); 445 | EEPROM.write(28,PWMrev2); 446 | EEPROM.write(29,PWMrev3); 447 | 448 | 449 | EEPROM.write(31,CutoffLimitMin3); 450 | EEPROM.write(32,InputClipMin3); 451 | 452 | EEPROM.write(35,DeadZone3); 453 | WriteEEPRomWord(36,Kp3_x100); 454 | WriteEEPRomWord(38,Ki3_x100); 455 | WriteEEPRomWord(40,Kd3_x100); 456 | EEPROM.write(42,PWMoffset3); 457 | EEPROM.write(43,PWMmax3); 458 | 459 | WriteEEPRomWord(44,Ks1); 460 | WriteEEPRomWord(46,Ks2); 461 | WriteEEPRomWord(48,Ks3); 462 | 463 | EEPROM.write(50,constrain(PIDProcessDivider,1,10)); 464 | EEPROM.write(51,Timer1FreqkHz); 465 | EEPROM.write(52,Timer2FreqkHz); 466 | } 467 | 468 | //**************************************************************************************************************** 469 | // Read all configurable parameters from the EEPROM. 470 | // 471 | //**************************************************************************************************************** 472 | 473 | 474 | void ReadEEProm() 475 | { 476 | int evalue = EEPROM.read(0); 477 | if(evalue != 114) //EEProm was not set before, set default values 478 | { 479 | WriteEEProm(); 480 | return; 481 | } 482 | CutoffLimitMin1 = EEPROM.read(1); 483 | InputClipMin1 = EEPROM.read(2); 484 | CutoffLimitMax1 = 1023-CutoffLimitMin1; 485 | InputClipMax1 = 1023-InputClipMin1; 486 | 487 | DeadZone1=EEPROM.read(5); 488 | CutoffLimitMin2 = EEPROM.read(6); 489 | InputClipMin2 = EEPROM.read(7); 490 | CutoffLimitMax2 = 1023-CutoffLimitMin2; 491 | InputClipMax2 = 1023-InputClipMin2; 492 | 493 | DeadZone2=EEPROM.read(10); 494 | Kp1_x100=ReadEEPRomWord(11); 495 | Ki1_x100=ReadEEPRomWord(13); 496 | Kd1_x100=ReadEEPRomWord(15); 497 | Kp2_x100=ReadEEPRomWord(17); 498 | Ki2_x100=ReadEEPRomWord(19); 499 | Kd2_x100=ReadEEPRomWord(21); 500 | PWMoffset1=EEPROM.read(23); 501 | PWMoffset2=EEPROM.read(24); 502 | PWMmax1=EEPROM.read(25); 503 | PWMmax2=EEPROM.read(26); 504 | PWMrev1=EEPROM.read(27); 505 | PWMrev2=EEPROM.read(28); 506 | PWMrev3=EEPROM.read(29); 507 | 508 | 509 | CutoffLimitMin3 = EEPROM.read(31); 510 | InputClipMin3 = EEPROM.read(32); 511 | CutoffLimitMax3 = 1023-CutoffLimitMin3; 512 | InputClipMax3 = 1023-InputClipMin3; 513 | 514 | DeadZone3=EEPROM.read(35); 515 | Kp3_x100=ReadEEPRomWord(36); 516 | Ki3_x100=ReadEEPRomWord(38); 517 | 518 | Kd3_x100=ReadEEPRomWord(40); 519 | PWMoffset3=EEPROM.read(42); 520 | PWMmax3=EEPROM.read(43); 521 | Ks1=ReadEEPRomWord(44); 522 | Ks2=ReadEEPRomWord(46); 523 | Ks3=ReadEEPRomWord(48); 524 | 525 | PIDProcessDivider=EEPROM.read(50); 526 | PIDProcessDivider=constrain(PIDProcessDivider,1,10); 527 | Timer1FreqkHz=EEPROM.read(51); 528 | Timer2FreqkHz=EEPROM.read(52); 529 | } 530 | 531 | 532 | 533 | //**************************************************************************************************************** 534 | // Send two bytes via serial in response to a request for information 535 | // 536 | //**************************************************************************************************************** 537 | 538 | void SendTwoValues(int id, int v1, int v2, int ComPort) 539 | { 540 | if (ComPort==0) 541 | { 542 | Serial.write(START_BYTE); 543 | Serial.write(id); 544 | Serial.write(v1); 545 | Serial.write(v2); 546 | Serial.write(END_BYTE); 547 | } 548 | else 549 | { 550 | #ifdef SECOND_SERIAL 551 | mySerial.write(START_BYTE); 552 | mySerial.write(id); 553 | mySerial.write(v1); 554 | mySerial.write(v2); 555 | mySerial.write(END_BYTE); 556 | #endif 557 | } 558 | } 559 | 560 | 561 | //**************************************************************************************************************** 562 | // Send a single word value via serial in response to a request for information 563 | // 564 | //**************************************************************************************************************** 565 | 566 | void SendValue(int id, int value, int ComPort) 567 | { 568 | int low,high; 569 | 570 | high=value/256; 571 | low=value-(high*256); 572 | 573 | if (ComPort==0) 574 | { 575 | Serial.write(START_BYTE); 576 | Serial.write(id); 577 | Serial.write(high); 578 | Serial.write(low); 579 | Serial.write(END_BYTE); 580 | } 581 | else 582 | { 583 | #ifdef SECOND_SERIAL 584 | mySerial.write(START_BYTE); 585 | mySerial.write(id); 586 | mySerial.write(high); 587 | mySerial.write(low); 588 | mySerial.write(END_BYTE); 589 | #endif 590 | } 591 | } 592 | 593 | 594 | //**************************************************************************************************************** 595 | // Calculate how many PID calculations have been performed since last requested 596 | // 597 | //**************************************************************************************************************** 598 | 599 | int DeltaLoopCount() 600 | { 601 | unsigned long Delta; 602 | 603 | if ( (LastCount==0) || ((LoopCount-LastCount)>32000) ) 604 | { 605 | Delta = 0; 606 | LastCount = LoopCount; 607 | } 608 | else 609 | { 610 | Delta = LoopCount-LastCount; 611 | LastCount = LoopCount; 612 | } 613 | return (int)Delta; 614 | } 615 | 616 | 617 | //**************************************************************************************************************** 618 | // Process the incoming serial commands from the specified comm port 619 | // 620 | //**************************************************************************************************************** 621 | 622 | 623 | void ParseCommand(int ComPort) 624 | { 625 | CommsTimeout = 0; // reset the comms timeout counter to indicate we are getting packets 626 | 627 | switch (RxBuffer[0][ComPort]) 628 | { 629 | case 'A': 630 | Target1=(RxBuffer[1][ComPort]*256)+RxBuffer[2][ComPort]; 631 | #ifdef REVERSE_MOTOR1 632 | Target1=1023-Target1; 633 | #endif 634 | 635 | #ifdef ENABLE_POT_SCALING 636 | if (PotInput < 25) 637 | { 638 | Target1 = 512; // Center Motor1 639 | } 640 | else if (PotInput < 1000) 641 | { 642 | Target1 = int(float(Target1-512) * (0.0 + float(PotInput) * 0.001)) + 512; 643 | } 644 | #endif 645 | 646 | #ifdef ENABLE_NON_LINEAR_POT_SCALING 647 | if (PotInput < 25) 648 | { 649 | Target1 = 512; // Center Motor1 650 | } 651 | else if (PotInput < 512) 652 | { 653 | Target1 = (Target1 - 512) << 1; 654 | if (Target1 < 0) 655 | { 656 | Target1 = int((-Target1 - float((double(Target1) * double(Target1))) * 0.000488) * float(PotInput * 0.001953)); 657 | Target1 = (-Target1 >> 1) + 512; 658 | } 659 | else 660 | { 661 | Target1 = int((Target1 - float((double(Target1) * double(Target1))) * 0.000488) * float(PotInput * 0.001953)); 662 | Target1 = (Target1 >> 1) + 512; 663 | } 664 | } 665 | else if (PotInput < 1000) 666 | { 667 | Target1 = (Target1 - 512) << 1; 668 | if (Target1 < 0) 669 | { 670 | Target1 = int(-Target1 - float((double(Target1) * double(Target1) * double(1024 - PotInput))) * 0.000000953674); 671 | Target1= (-Target1 >> 1) + 512; 672 | } 673 | else 674 | { 675 | Target1 = int(Target1 - float((double(Target1) * double(Target1) * double(1024 - PotInput))) * 0.000000953674); 676 | Target1= (Target1 >> 1) + 512; 677 | } 678 | } 679 | #endif 680 | 681 | if (Target1>InputClipMax1) { Target1=InputClipMax1; } 682 | else if (Target1> 1) + 512; 709 | } 710 | else 711 | { 712 | Target2 = int((Target2 - float((double(Target2) * double(Target2))) * 0.000488) * float(PotInput * 0.001953)); 713 | Target2 = (Target2 >> 1) + 512; 714 | } 715 | } 716 | else if (PotInput < 1000) 717 | { 718 | Target2 = (Target2 - 512) << 1; 719 | if (Target2 < 0) 720 | { 721 | Target2 = int(-Target2 - float((double(Target2) * double(Target2) * double(1024 - PotInput))) * 0.000000953674); 722 | Target2= (-Target2 >> 1) + 512; 723 | } 724 | else 725 | { 726 | Target2 = int(Target2 - float((double(Target2) * double(Target2) * double(1024 - PotInput))) * 0.000000953674); 727 | Target2= (Target2 >> 1) + 512; 728 | } 729 | } 730 | #endif 731 | 732 | if (Target2>InputClipMax2) { Target2=InputClipMax2; } 733 | else if (Target2524) 745 | { 746 | Target3 = 524; 747 | } 748 | #endif 749 | if (Target3>InputClipMax3) { Target3=InputClipMax3; } 750 | else if (Target3 3) 1057 | { 1058 | if(RxBuffer[3][COM0]==END_BYTE) 1059 | { 1060 | ParseCommand(COM0); 1061 | } 1062 | else 1063 | { 1064 | errorcount++; 1065 | } 1066 | BufferEnd[COM0]=-1; 1067 | } 1068 | } 1069 | } 1070 | } 1071 | 1072 | 1073 | void CheckSerial1() 1074 | { 1075 | #ifdef SECOND_SERIAL 1076 | while(mySerial.available()) 1077 | { 1078 | if(BufferEnd[COM1]==-1) 1079 | { 1080 | RxByte[COM1] = mySerial.read(); 1081 | if(RxByte[COM1] != START_BYTE){BufferEnd[COM1]=-1;}else{BufferEnd[COM1]=0;} 1082 | } 1083 | else 1084 | { 1085 | RxByte[COM1] = mySerial.read(); 1086 | RxBuffer[BufferEnd[COM1]][COM1]=RxByte[COM1]; 1087 | BufferEnd[COM1]++; 1088 | if(BufferEnd[COM1] > 3) 1089 | { 1090 | if(RxBuffer[3][COM1]==END_BYTE) 1091 | { 1092 | ParseCommand(COM1); 1093 | } 1094 | else 1095 | { 1096 | errorcount++; 1097 | } 1098 | BufferEnd[COM1]=-1; 1099 | } 1100 | } 1101 | } 1102 | #endif 1103 | } 1104 | 1105 | 1106 | #ifdef MODE1 // This mode outputs a PWM and two motor Direction pins 1107 | 1108 | //**************************************************************************************************************** 1109 | // Set the Motor output pins to drive the motor in required direction and speed 1110 | // 1111 | //**************************************************************************************************************** 1112 | 1113 | 1114 | void SetOutputsMotor1() 1115 | { 1116 | if((Feedback1 > InputClipMax1) && (PWMrev1 != 0)) 1117 | { 1118 | PWMout1 = PWMrev1; 1119 | OutputPort &= ~(1 << ENApin1); // Unset Motor1 In 1 1120 | OutputPort |= 1 << ENBpin1; // Set Motor1 In 2 1121 | MyPWMWrite(PWMpin1, PWMrev1); 1122 | } 1123 | else if((Feedback1 (Feedback1 + DeadZone1)) || (Target1 < (Feedback1 - DeadZone1))) 1131 | { 1132 | if (PWMout1 >= 0) 1133 | { 1134 | // Drive Motor Forward 1135 | PWMout1+=PWMoffset1; 1136 | if(PWMout1 > (PWMmax1+LiftFactor1)){PWMout1=PWMmax1+LiftFactor1;} 1137 | OutputPort |= 1 << ENApin1; // Set Motor1 In 1 1138 | OutputPort &= ~(1 << ENBpin1); // Unset Motor1 In 2 1139 | } 1140 | else 1141 | { 1142 | // Drive Motor Backwards 1143 | PWMout1 = abs(PWMout1); 1144 | PWMout1+=PWMoffset1; 1145 | if(PWMout1 > PWMmax1){PWMout1=PWMmax1;} 1146 | OutputPort &= ~(1 << ENApin1); // Unset Motor1 In 1 1147 | OutputPort |= 1 << ENBpin1; // Set Motor1 In 2 1148 | } 1149 | MyPWMWrite(PWMpin1, PWMout1); 1150 | } 1151 | else 1152 | { 1153 | // Brake Motor 1154 | OutputPort &= ~(1 << ENApin1); // Unset Motor1 In 1 1155 | OutputPort &= ~(1 << ENBpin1); // Unset Motor1 In 2 1156 | PWMout1=PWMoffset1; 1157 | MyPWMWrite(PWMpin1, 0); 1158 | } 1159 | PORTD = OutputPort; 1160 | } 1161 | 1162 | 1163 | void SetOutputsMotor2() 1164 | { 1165 | if((Feedback2 > InputClipMax2) && (PWMrev2 != 0)) 1166 | { 1167 | PWMout2 = PWMrev2; 1168 | OutputPort &= ~(1 << ENApin2); // Unset Motor1 In 1 1169 | OutputPort |= 1 << ENBpin2; // Set Motor1 In 2 1170 | MyPWMWrite(PWMpin2, PWMrev2); 1171 | } 1172 | else if((Feedback2 (Feedback2 + DeadZone2) || Target2 < (Feedback2 - DeadZone2)) 1180 | { 1181 | if (PWMout2 >= 0) 1182 | { 1183 | // Drive Motor Forward 1184 | PWMout2+=PWMoffset2; 1185 | if(PWMout2 > PWMmax2+LiftFactor2){PWMout2=PWMmax2+LiftFactor2;} 1186 | OutputPort |= 1 << ENApin2; // Set Motor2 In 1 1187 | OutputPort &= ~(1 << ENBpin2); // Unset Motor2 In 2 1188 | } 1189 | else 1190 | { 1191 | // Drive Motor Backwards 1192 | PWMout2 = abs(PWMout2); 1193 | PWMout2+=PWMoffset2; 1194 | if(PWMout2 > PWMmax2){PWMout2=PWMmax2;} 1195 | OutputPort &= ~(1 << ENApin2); // Unset Motor2 In 1 1196 | OutputPort |= 1 << ENBpin2; // Set Motor2 In 2 1197 | } 1198 | MyPWMWrite(PWMpin2, PWMout2); 1199 | } 1200 | else 1201 | { 1202 | // Brake Motor 1203 | OutputPort &= ~(1 << ENApin2); // Unset Motor2 In 1 1204 | OutputPort &= ~(1 << ENBpin2); // Unset Motor2 In 2 1205 | PWMout2=PWMoffset2; 1206 | MyPWMWrite(PWMpin2, 0); 1207 | } 1208 | PORTD = OutputPort; 1209 | } 1210 | 1211 | 1212 | void SetOutputsMotor3() 1213 | { 1214 | if((Feedback3 > InputClipMax3) && (PWMrev3 != 0)) 1215 | { 1216 | PWMout3 = PWMrev3; 1217 | OutputPort &= ~(1 << ENApin3); // Unset Motor1 In 1 1218 | OutputPort |= 1 << ENBpin3; // Set Motor1 In 2 1219 | analogWrite(PWMpin3, PWMrev3); 1220 | } 1221 | else if((Feedback3 (Feedback3 + DeadZone3) || Target3 < (Feedback3 - DeadZone3)) 1229 | { 1230 | if (PWMout3 >= 0) 1231 | { 1232 | // Drive Motor Forward 1233 | PWMout3+=PWMoffset3; 1234 | if(PWMout3> PWMmax3){PWMout3=PWMmax3;} 1235 | OutputPort |= 1 << ENApin3; // Set Motor3 In 1 1236 | OutputPort &= ~(1 << ENBpin3); // Unset Motor3 In 2 1237 | } 1238 | else 1239 | { 1240 | // Drive Motor Backwards 1241 | PWMout3 = abs(PWMout3); 1242 | PWMout3+=PWMoffset3; 1243 | if(PWMout3> PWMmax3){PWMout3=PWMmax3;} 1244 | OutputPort &= ~(1 << ENApin3); // Unset Motor3 In 1 1245 | OutputPort |= 1 << ENBpin3; // Set Motor3 In 2 1246 | } 1247 | analogWrite(PWMpin3, PWMout3); 1248 | } 1249 | else 1250 | { 1251 | // Brake Motor 1252 | OutputPort &= ~(1 << ENApin3); // Unset Motor3 In 1 1253 | OutputPort &= ~(1 << ENBpin3); // Unset Motor3 In 2 1254 | PWMout3=PWMoffset3; 1255 | analogWrite(PWMpin3, 0); 1256 | } 1257 | PORTD = OutputPort; 1258 | } 1259 | 1260 | #endif 1261 | 1262 | 1263 | #ifdef MODE2 // This mode outputs a H-Bridge Enable, One Dir Pin and a +/-PWM 1264 | 1265 | //**************************************************************************************************************** 1266 | // Set the Motor output pins to drive the motor in required direction and speed 1267 | // 1268 | //**************************************************************************************************************** 1269 | 1270 | 1271 | void SetOutputsMotor1() // MODE2 1272 | { 1273 | if((Feedback1 > InputClipMax1) && (PWMrev1 != 0)) 1274 | { 1275 | PWMout1 = PWMrev1; 1276 | OutputPort &= ~(1 << ENApin1); // Unset Motor1 In 1 1277 | MyPWMWrite(PWMpin1, PWMrev1); 1278 | } 1279 | else if((Feedback1 (Feedback1 + DeadZone1)) || (Target1 < (Feedback1 - DeadZone1))) 1286 | { 1287 | if (PWMout1 >= 0) 1288 | { 1289 | // Drive Motor Forward 1290 | PWMout1+=PWMoffset1; 1291 | if(PWMout1 > (PWMmax1+LiftFactor1)){PWMout1=PWMmax1+LiftFactor1;} 1292 | OutputPort |= 1 << ENApin1; // Set Motor1 In 1 1293 | MyPWMWrite(PWMpin1, 255-PWMout1); // Motor driven when PWM=0 (unset) 1294 | } 1295 | else 1296 | { 1297 | // Drive Motor Backwards 1298 | PWMout1 = abs(PWMout1); 1299 | PWMout1+=PWMoffset1; 1300 | if(PWMout1 > PWMmax1){PWMout1=PWMmax1;} 1301 | OutputPort &= ~(1 << ENApin1); // Unset Motor1 In 1 1302 | MyPWMWrite(PWMpin1, PWMout1); // Motor driven when PWM=1 (set) 1303 | } 1304 | } 1305 | else 1306 | { 1307 | // Brake Motor 1308 | OutputPort &= ~(1 << ENApin1); // Unset Motor1 In 1 1309 | PWMout1=PWMoffset1; 1310 | MyPWMWrite(PWMpin1, 0); 1311 | } 1312 | PORTD = OutputPort; 1313 | } 1314 | 1315 | 1316 | void SetOutputsMotor2() // MODE2 1317 | { 1318 | if ((Feedback2>InputClipMax2) && (PWMrev2 != 0)) 1319 | { 1320 | PWMout2 = PWMrev2; 1321 | OutputPort &= ~(1 << ENApin2); // Unset Motor2 In 1 1322 | MyPWMWrite(PWMpin2, PWMrev2); 1323 | } 1324 | else if ((Feedback2 (Feedback2 + DeadZone2)) || (Target2 < (Feedback2 - DeadZone2))) 1331 | { 1332 | if (PWMout2 >= 0) 1333 | { 1334 | // Drive Motor Forward 1335 | PWMout2+=PWMoffset2; 1336 | if(PWMout2 > PWMmax2+LiftFactor2){PWMout2=PWMmax2+LiftFactor2;} 1337 | OutputPort |= 1 << ENApin2; // Set Motor2 In 1 1338 | MyPWMWrite(PWMpin2, 255-PWMout2); 1339 | } 1340 | else 1341 | { 1342 | // Drive Motor Backwards 1343 | PWMout2 = abs(PWMout2); 1344 | PWMout2+=PWMoffset2; 1345 | if(PWMout2 > PWMmax2){PWMout2=PWMmax2;} 1346 | OutputPort &= ~(1 << ENApin2); // Unset Motor2 In 1 1347 | MyPWMWrite(PWMpin2, PWMout2); 1348 | } 1349 | } 1350 | else 1351 | { 1352 | // Brake Motor 1353 | OutputPort &= ~(1 << ENApin2); // Unset Motor2 In 1 1354 | PWMout2=PWMoffset2; 1355 | MyPWMWrite(PWMpin2, 0); 1356 | } 1357 | PORTD = OutputPort; 1358 | } 1359 | 1360 | 1361 | void SetOutputsMotor3() // MODE2 1362 | { 1363 | if ((Feedback3>InputClipMax3) && (PWMrev3 != 0)) 1364 | { 1365 | PWMout3 = PWMrev3; 1366 | OutputPort &= ~(1 << ENApin3); // Unset Motor3 In 1 1367 | analogWrite(PWMpin3, PWMrev3); 1368 | } 1369 | else if ((Feedback3 (Feedback3 + DeadZone3)) || (Target3 < (Feedback3 - DeadZone3))) 1376 | { 1377 | if (PWMout3 >= 0) 1378 | { 1379 | // Drive Motor Forward 1380 | PWMout3+=PWMoffset3; 1381 | if(PWMout3> PWMmax3){PWMout3=PWMmax3;} 1382 | OutputPort |= 1 << ENApin3; // Set Motor3 In 1 1383 | analogWrite(PWMpin3, 255-PWMout3); 1384 | } 1385 | else 1386 | { 1387 | // Drive Motor Backwards 1388 | PWMout3 = abs(PWMout3); 1389 | PWMout3+=PWMoffset3; 1390 | if(PWMout3> PWMmax3){PWMout3=PWMmax3;} 1391 | OutputPort &= ~(1 << ENApin3); // Unset Motor3 In 1 1392 | analogWrite(PWMpin3, PWMout3); 1393 | } 1394 | } 1395 | else 1396 | { 1397 | // Brake Motor 1398 | OutputPort &= ~(1 << ENApin3); // Unset Motor3 In 1 1399 | PWMout3=PWMoffset3; 1400 | analogWrite(PWMpin3, 0); 1401 | } 1402 | PORTD = OutputPort; 1403 | } 1404 | 1405 | #endif 1406 | 1407 | 1408 | 1409 | 1410 | //**************************************************************************************************************** 1411 | // Calculate the motor PID output 1412 | // Based on http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ 1413 | // Re-written to eliminate the use of float calculations to significantly increase calculation speed. 1414 | // 1415 | //**************************************************************************************************************** 1416 | 1417 | 1418 | 1419 | int CalcMotor1PID(int TargetPosition, int CurrentPosition) 1420 | { 1421 | static int Error=0; 1422 | static long pTerm_x100=0; 1423 | static long dTerm_x100=0; 1424 | static long iTerm_x100=0; 1425 | static int CumError=0; 1426 | static int LastPosition=0; 1427 | static int DeltaPosition=0; 1428 | static int KdFilterCount=0; 1429 | 1430 | Error = TargetPosition - CurrentPosition; 1431 | if (abs(Error)<=DeadZone1) 1432 | { 1433 | CumError = 0; 1434 | } 1435 | else 1436 | { 1437 | CumError += Error; 1438 | CumError = constrain(CumError,-1024,1024); 1439 | } 1440 | 1441 | pTerm_x100 = Kp1_x100 * (long)Error; // Error can only be between +/-1023 and Kp1_100 is constrained to 0-1000 so can work with type long 1442 | iTerm_x100 = (Ki1_x100 * (long)CumError); // was >>6 1443 | 1444 | KdFilterCount++; 1445 | if (KdFilterCount >= Ks1) // Apply a level of filtering to Kd parameter to help reduce motor noise 1446 | { 1447 | DeltaPosition = (CurrentPosition - LastPosition); 1448 | LastPosition = CurrentPosition; 1449 | dTerm_x100 = (Kd1_x100 * (long)DeltaPosition); // was <<5 1450 | KdFilterCount=0; 1451 | } 1452 | 1453 | return constrain((pTerm_x100 + iTerm_x100 - dTerm_x100) >> PowerScale ,-255,255); // the /128 (PowerScale) is an approximation to bring x100 terms back to units. Accurate/consistent enough for this function. 1454 | } 1455 | 1456 | int CalcMotor2PID(int TargetPosition, int CurrentPosition) 1457 | { 1458 | static int Error=0; 1459 | static long pTerm_x100=0; 1460 | static long dTerm_x100=0; 1461 | static long iTerm_x100=0; 1462 | static int CumError=0; 1463 | static int LastPosition=0; 1464 | static int DeltaPosition=0; 1465 | static int KdFilterCount=0; 1466 | 1467 | Error = TargetPosition - CurrentPosition; 1468 | if (abs(Error)<=DeadZone2) 1469 | { 1470 | CumError = 0; 1471 | } 1472 | else 1473 | { 1474 | CumError += Error; 1475 | CumError = constrain(CumError,-1024,1024); // Maybe Try 512 as the limits?????? 1476 | } 1477 | 1478 | DeltaPosition = (CurrentPosition - LastPosition); 1479 | LastPosition = CurrentPosition; 1480 | 1481 | 1482 | pTerm_x100 = Kp2_x100 * (long)Error; // Error can only be between +/-1023 and Kp1_100 is constrained to 0-1000 so can work with type long 1483 | iTerm_x100 = (Ki2_x100 * (long)CumError); 1484 | 1485 | KdFilterCount++; 1486 | if (KdFilterCount >= Ks2) // Apply a level of filtering to Kd parameter to help reduce motor noise 1487 | { 1488 | DeltaPosition = (CurrentPosition - LastPosition); 1489 | LastPosition = CurrentPosition; 1490 | dTerm_x100 = (Kd2_x100 * (long)DeltaPosition); 1491 | KdFilterCount=0; 1492 | } 1493 | 1494 | return constrain((pTerm_x100 + iTerm_x100 - dTerm_x100) >> PowerScale ,-255,255); // the /128 is an approximation to bring x100 terms back to units. Accurate/consistent enough for this function. 1495 | } 1496 | 1497 | int CalcMotor3PID(int TargetPosition, int CurrentPosition) 1498 | { 1499 | static int Error=0; 1500 | static long pTerm_x100=0; 1501 | static long dTerm_x100=0; 1502 | static long iTerm_x100=0; 1503 | static int CumError=0; 1504 | static int LastPosition=0; 1505 | static int DeltaPosition=0; 1506 | static int KdFilterCount=0; 1507 | 1508 | Error = TargetPosition - CurrentPosition; 1509 | if (abs(Error)<=DeadZone3) 1510 | { 1511 | CumError = 0; 1512 | } 1513 | else 1514 | { 1515 | CumError += Error; 1516 | CumError = constrain(CumError,-1024,1024); // Maybe Try 512 as the limits?????? 1517 | } 1518 | 1519 | DeltaPosition = (CurrentPosition - LastPosition); 1520 | LastPosition = CurrentPosition; 1521 | 1522 | 1523 | pTerm_x100 = Kp3_x100 * (long)Error; // Error can only be between +/-1023 and Kp1_100 is constrained to 0-1000 so can work with type long 1524 | iTerm_x100 = (Ki3_x100 * (long)CumError) >> 6; 1525 | 1526 | KdFilterCount++; 1527 | if (KdFilterCount >= Ks3) // Apply a level of filtering to Kd parameter to help reduce motor noise 1528 | { 1529 | DeltaPosition = (CurrentPosition - LastPosition); 1530 | LastPosition = CurrentPosition; 1531 | dTerm_x100 = (Kd3_x100 * (long)DeltaPosition)<<5; 1532 | KdFilterCount=0; 1533 | } 1534 | 1535 | return constrain((pTerm_x100 + iTerm_x100 - dTerm_x100) >> PowerScale ,-255,255); // the /128 is an approximation to bring x100 terms back to units. Accurate/consistent enough for this function. 1536 | } 1537 | 1538 | 1539 | 1540 | //**************************************************************************************************************** 1541 | // Disable the Motors - ie put the brakes on 1542 | // Used when motor feedback is outside allowed range to minimise damage 1543 | // 1544 | //**************************************************************************************************************** 1545 | 1546 | void DisableMotor1() 1547 | { 1548 | Disable1 = 1; 1549 | 1550 | OutputPort &= ~(1 << ENApin1); 1551 | OutputPort &= ~(1 << ENBpin1); 1552 | 1553 | PORTD = OutputPort; 1554 | 1555 | MyPWMWrite(PWMpin1, 0); 1556 | 1557 | } 1558 | 1559 | void DisableMotor2() 1560 | { 1561 | Disable2 = 1; 1562 | 1563 | OutputPort &= ~(1 << ENApin2); 1564 | OutputPort &= ~(1 << ENBpin2); 1565 | 1566 | PORTD = OutputPort; 1567 | 1568 | MyPWMWrite(PWMpin2, 0); 1569 | 1570 | } 1571 | 1572 | void DisableMotor3() 1573 | { 1574 | Disable3 = 1; 1575 | 1576 | OutputPort &= ~(1 << ENApin3); 1577 | OutputPort &= ~(1 << ENBpin3); 1578 | 1579 | PORTD = OutputPort; 1580 | 1581 | analogWrite(PWMpin3, 0); 1582 | 1583 | } 1584 | 1585 | 1586 | void TogglePin() 1587 | { 1588 | static int PinOut=0; 1589 | PinOut=1-PinOut; 1590 | digitalWrite(8, PinOut); 1591 | } 1592 | 1593 | 1594 | //**************************************************************************************************************** 1595 | // Main Arduino Program Loop 1596 | // 1597 | //**************************************************************************************************************** 1598 | 1599 | void loop() 1600 | { 1601 | 1602 | 1603 | // Enable all motors 1604 | 1605 | Disable1=0; 1606 | Disable2=0; 1607 | Disable3=0; 1608 | 1609 | #ifdef MODE2 // 43A "Chinese" H-Bridge 1610 | // Reset any overtemp shutdowns and enable the H-Bridges - toggle pins low then leave high 1611 | OutputPort &= ~(1 << ENBpin1); // Unset Motor1 In 2 1612 | OutputPort &= ~(1 << ENBpin2); // Unset Motor2 In 2 1613 | OutputPort &= ~(1 << ENBpin3); // Unset Motor3 In 2 1614 | PORTD = OutputPort; 1615 | OutputPort |= 1 << ENBpin1; // Set Motor1 In 2 1616 | OutputPort |= 1 << ENBpin2; // Set Motor2 In 2 1617 | OutputPort |= 1 << ENBpin3; // Set Motor3 In 2 1618 | PORTD = OutputPort; 1619 | #endif 1620 | 1621 | 1622 | // Some temporary debug stuff 1623 | // Serial.write('S'); 1624 | // Serial.write(TCCR1A); 1625 | // Serial.write(TCCR1B); 1626 | // Serial.write(OCR1AL); 1627 | // Serial.write(OCR1AH); 1628 | 1629 | 1630 | 1631 | 1632 | 1633 | 1634 | // Initialise the PID ready timer 1635 | 1636 | TimesUp = micros(); 1637 | PIDProcessCounter=0; 1638 | 1639 | // Main Program loop 1640 | 1641 | while (1==1) 1642 | { 1643 | 1644 | // Wait until its time and then update PID calcs for first motor 1645 | 1646 | while ((micros() - TimesUp) < PROCESS_PERIOD_uS) { ; } 1647 | TimesUp += PROCESS_PERIOD_uS; 1648 | TogglePin(); // Used for testing to monitor PID timing on Oscilloscope 1649 | 1650 | PIDProcessCounter++; 1651 | 1652 | if (PIDProcessCounter >= PIDProcessDivider) 1653 | { 1654 | PIDProcessCounter=0; 1655 | 1656 | // Check and Update Motor 1 drive 1657 | 1658 | Feedback1 = analogRead(FeedbackPin1); 1659 | if ((Feedback1 > CutoffLimitMax1) || (Feedback1 < CutoffLimitMin1)) { DisableMotor1(); } 1660 | PWMout1=CalcMotor1PID(Target1,Feedback1); 1661 | if (Disable1==0) 1662 | { 1663 | SetOutputsMotor1(); 1664 | } 1665 | else 1666 | { 1667 | PWMout1=0; 1668 | } 1669 | 1670 | // Check and Update Motor 2 drive 1671 | 1672 | Feedback2 = analogRead(FeedbackPin2); 1673 | if ((Feedback2 > CutoffLimitMax2) || (Feedback2 < CutoffLimitMin2)) { DisableMotor2(); } 1674 | PWMout2=CalcMotor2PID(Target2,Feedback2); 1675 | if (Disable2==0) 1676 | { 1677 | SetOutputsMotor2(); 1678 | } 1679 | else 1680 | { 1681 | PWMout2=0; 1682 | } 1683 | 1684 | // Check and Update Motor 3 drive 1685 | 1686 | Feedback3 = analogRead(FeedbackPin3); 1687 | if ((Feedback3 > CutoffLimitMax3) || (Feedback3 < CutoffLimitMin3)) { DisableMotor3(); } 1688 | PWMout3=CalcMotor3PID(Target3,Feedback3); 1689 | if (Disable3==0) 1690 | { 1691 | SetOutputsMotor3(); 1692 | } 1693 | else 1694 | { 1695 | PWMout3=0; 1696 | } 1697 | 1698 | LoopCount++; 1699 | } 1700 | 1701 | // Check if there is any received serial data and process (Hardware UART) 1702 | 1703 | CheckSerial0(); 1704 | 1705 | // Check if there is any received serial data and process (Software Serial) 1706 | 1707 | CheckSerial1(); 1708 | 1709 | SerialFeedbackCounter++; 1710 | if (SerialFeedbackCounter >= 80) // every 20ms send back position, pwm and motor status updates if enabled 1711 | { 1712 | SerialFeedbackPort = (SerialFeedbackEnabled >> 4) & 0x01; 1713 | if ((SerialFeedbackEnabled & 0x03) == 1) // Monitor Motor 1 1714 | { 1715 | #ifdef REVERSE_MOTOR1 1716 | SendTwoValues('A',(1023-Feedback1)/4,(1023-Target1)/4,SerialFeedbackPort); 1717 | #else 1718 | SendTwoValues('A',Feedback1/4,Target1/4,SerialFeedbackPort); 1719 | #endif 1720 | SendTwoValues('a',PIDProcessDivider*16 + Disable1+(Disable2*2)+(Disable3*4),constrain(PWMout1,0,255),SerialFeedbackPort); 1721 | } 1722 | else if ((SerialFeedbackEnabled & 0x03) == 2) // Monitor Motor 2 1723 | { 1724 | SendTwoValues('B',Feedback2/4,Target2/4,SerialFeedbackPort); 1725 | SendTwoValues('b',PIDProcessDivider*16 + Disable1+(Disable2*2)+(Disable3*4),constrain(PWMout2,0,255),SerialFeedbackPort); 1726 | } 1727 | else if ((SerialFeedbackEnabled & 0x03) == 3) // Monitor Motor 3 1728 | { 1729 | SendTwoValues('C',Feedback3/4,Target3/4,SerialFeedbackPort); 1730 | SendTwoValues('c',PIDProcessDivider*16 + Disable1+(Disable2*2)+(Disable3*4),constrain(PWMout3,0,255),SerialFeedbackPort); 1731 | } 1732 | SerialFeedbackCounter = 0; 1733 | 1734 | #ifdef ENABLE_POT_SCALING 1735 | PotInput = PotInput * 0.9; 1736 | PotInput = PotInput + (0.1 * analogRead(PotInputPin)); // Also read the User Pot Input every 20ms 1737 | #endif 1738 | 1739 | #ifdef ENABLE_NON_LINEAR_POT_SCALING 1740 | PotInput = PotInput * 0.9; 1741 | PotInput = PotInput + (0.1 * analogRead(PotInputPin)); // Also read the User Pot Input every 20ms 1742 | #endif 1743 | 1744 | } 1745 | 1746 | CommsTimeout++; 1747 | if (CommsTimeout >= 60000) // 15 second timeout ie 60000 * PROCESS_PERIOD_uS 1748 | { 1749 | CommsTimeout = 60000; // So the counter doesn't overflow 1750 | PowerScale = 9; 1751 | } 1752 | else 1753 | { 1754 | PowerScale = 7; // Number of bits to shift PID result (ie divide by 128 - approximate for /100) 1755 | } 1756 | 1757 | 1758 | } 1759 | } 1760 | --------------------------------------------------------------------------------