Please help arduino

Other Hardware Motors, Mechanics, Power and CNC
1

Hi
Im really struggling to understand whats going wrong
Here is a summary
I am using an arduino to control 3 motors on a motion simulator for gaming
I was using elegoo clone with smc3 sketch all was fine till i had a short and pin a0 was just producing noise
So i purchased a genuine arduino
The clone showed green and orange lights and showed up as arduino on comm port 3

The genuine is showing 2 red lights 1 flashing and shows up as serial usb on com port 4

I try to write the smc3 script and it says com port 4 access denied
I can copy the error and paste when i open up my pc
Please somebody help me understand im stuck after doing so well
Kind regards
Maty

2

Begin by reading How to get the best out of this forum and following the advice contained therein. Once you have provided the necessary information, I imagine someone will be able to help. But you have to do to your part first.

1 Like
3

After you finish reading Post #2...

Does the operating system recognize any device plugged into that USB port?

4

Yes in the IDE it shows arduino uno port com 4
tools port com4 is ticked
device manager says usb serial port (COM4)
the boards lights are 1 red 1 flashing red

now if i plug in the r3 i get orange and green light
ide shows port com3
device manager says arduino uno port com3
it works but the pin a0 has a problem just shows noise so cant use potentiometre
thats why i got a new board

the r3 has square usb port like printer type the r4 has usbc

if i can fix the r3 pin a0 problem would be better as i can upload to it

when i try to upload to the r4 i get :

Sketch uses 10788 bytes (33%) of program storage space. Maximum is 32256 bytes.
Global variables use 389 bytes (18%) of dynamic memory, leaving 1659 bytes for local variables. Maximum is 2048 bytes.
avrdude: ser_open(): can't open device "\.\COM4": Access is denied.

Failed uploading: uploading error: exit status 1

5

Before asking questions:

  • We often ask questions before offering solutions.

Please respond with the appropriate information requested.

  • Questioners know what their hardware and software looks like but the volunteers here do not. You need to fully explain what’s not working, and how it is supposed to work.
  • Before asking your questions, please read all the posting guidelines, follow these guidelines when you post your questions.

Hardware

  • Schematic diagrams are the universal language of electronics.
  • We use schematics to communicate hardware design. All components and connections are illustrated.
  • As always, show us a good schematic of your proposed circuit. Hand drawn schematics are acceptable.
  • Show us several good image views of your actual wiring.
  • Give WEB links to your major components.

Software

  • For readability, place { and } on separate lines by themselves, place each line of code on a separate line.
  • In the Arduino IDE, use Ctrl T or CMD T to format your code, then copy the complete sketch.
  • Use the < CODE / > icon from the ‘posting menu’ to attach your copied sketch
  • Examining a variable for HIGH/LOW or true/false makes your code very difficult to follow.
  • Avoid magic numbers in your sketches.
  • Something like this documents your code for you and tells others what is happening:

#define ENABLED true

#define DISABLED false

//

#define CW 0xB847FF00

#define CCW 0xB847FFA3

//

#define LEDon HIGH

#define LEDoff LOW

//

#define PRESSED HIGH

#define RELEASED LOW

  • When you follow the posting guidelines, volunteers can be more effective.
  • When we have a schematic we can speak the schematic language in helping you.
  • More volunteers will help when you properly post your questions.
6

I see you have not finished reading Post #2. You should.

The R3 green LED should have the label "POWER" nearby. What label is near the "orange" LED?

Good, but how do you determine it is working?

I use A0 because it has "noise"... but because you did not read Post #2, you do not know to include a wiring diagram and your program code that tells you the pin is not good. Include those here.

That makes me ask again, "How do you determine 'it works' when the COM port will not let you communicate with the R3?"
... and read Post #2.

7

Hi again
thanx for replying i will try to uploa what you need
but let me try to explain before i do

the r3 shows up fine on com3 and is showing orange and green lights i can write to it and it can run the motors fine its just the pin a0 is failing to read the potentiometre

this is why ive purchased the new R4 but this new board which is arduino only shows as serial usb it shows on com 4 in device manager and in ide
i try to upload to the r4 and it gives me this message :

avrdude: ser_open(): can't open device "\.\COM4": Access is denied.

Failed uploading: uploading error: exit status 1

I will continue to read through Post #2 but at the moment its way over my head

please bare with me

8

Show the wiring diagram and sketch for this.

You should treat the R4 as a different topic... to keep this topic for the R3.

9

also on r3 green is on and orange is L

r4 red is on flashing red is L

i dnt understand {and}
and ctr and T does nothing in ide

i can copy and paste sketch if you want or theres a copy button when it fails to upload sketch to R4

10

Ok that might be a better solution although the origional topic was R4
But if i can fix the R3 it will be better as the sketch is already on yhere and it works fine apart fom pin a0
ill upload sketch now

11 
//****************************************************************************************************************
// SMC3.ini is basic Motor PID driver designed for motion simulators with upto 3 motors (written for UNO R3)
//
//****************************************************************************************************************

// Set to MODE1 for use with a typical H-Bride that requires PWM and 1 or 2 direction inputs
// Set to MODE2 for a 43A "Chinese" IBT-2 H-Bridge from e-bay or equiv

#define MODE2

// Uncomment the following line to reverse the direction of Motor 1.

//#define REVERSE_MOTOR1

// Uncomment ONE of the following lines to enable analogue input AN5 as a scaler for the motion values.

// #define ENABLE_POT_SCALING
// #define ENABLE_NON_LINEAR_POT_SCALING

// Uncomment the following line to enable the second software serial port.
// NOTE: This is currently not working - leave commented out until fixed!!!

// #define SECOND_SERIAL

//    COMMAND SET:
//
//    []              		Drive all motors to defined stop positions and hold there
//    [Axx],[Bxx],[Cxx]         Send position updates for Motor 1,2,3 where xx is the binary position limitted to range 0-1024
//    [Dxx],[Exx],[Fxx]		Send the Kp parameter for motor 1,2,3 where xx is the Kp value multiplied by 100
//    [Gxx],[Hxx],[Ixx]		Send the Ki parameter for motor 1,2,3 where xx is the Ki value multiplied by 100
//    [Jxx],[Kxx],[Lxx]		Send the Kd parameter for motor 1,2,3 where xx is the Kd value multiplied by 100
//    [Mxx],[Nxx],[Oxx]		Send the Ks parameter for motor 1,2,3 where xx is the Ks value
//    [Pxy],[Qxy],[Rxy]		Send the PWMmin and PWMmax values x is the PWMmin and y is PWMmax each being in range 0-255
//    [Sxy],[Txy],[Uxy]         Send the Motor Min/Max Limits (x) and Input Min/Max Limits (y) (Note same value used for Min and Max)
//    [Vxy],[Wxy],[Xxy]         Send the Feedback dead zone (x) and the PWM reverse duty (y) for each motor
//    [rdx]          		Read a value from the controller where x is the code for the parameter to read
//    [ena]			Enable all motors
//    [sav]			Save parameters to non-volatile memory
//    [ver]                     Send the SMC3 software version
//

//    Arduino UNO Pinouts Used
//
//    9 - Motor 1 PWM
//    10 - Motor 2 PWM
//    11 - Motor 3 PWM
//    2 - Motor 1 H-Bridge ENA
//    3 - Motor 1 H-Bridge ENB
//    4 - Motor 2 H-Bridge ENA
//    5 - Motor 2 H-Bridge ENB
//    6 - Motor 3 H-Bridge ENA
//    7 - Motor 3 H-Bridge ENB
//    A0 - Motor 1 Feedback
//    A1 - Motor 2 Feedback
//    A2 - Motor 3 Feedback
//    A5 - Motion scaler pot input



// BOF preprocessor bug prevention - leave me at the top of the arduino-code
#if 1
__asm volatile("nop");
#endif

#include <EEPROM.h>
#include <SoftwareSerial.h>

// defines for setting and clearing register bits
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif

#define COM0 0                        // hardware Serial Port
#define COM1 1                        // Software Serial Port
#define START_BYTE '['                // Start Byte for serial commands
#define END_BYTE ']'                  // End Byte for serial commands
#define PROCESS_PERIOD_uS 250         // 244 -> 4096 per second approx
#define TIMER_UPPER_LIMIT 4294966000  // Used to check for timer wraparound
#define TIMER_LOWER_LIMIT 1000        // Used to check for timer wraparound


unsigned long TimesUp = 0;  // Counter used to see if it's time to calculate next PID update

int Feedback1 = 512;
int Feedback2 = 512;
int Feedback3 = 512;
int Target1 = 512;
int Target2 = 512;
int Target3 = 512;
int PotInput = 512;

unsigned int RxByte[2] = { 0 };       // Current byte received from each of the two comm ports
int BufferEnd[2] = { -1 };            // Rx Buffer end index for each of the two comm ports
unsigned int RxBuffer[5][2] = { 0 };  // 5 byte Rx Command Buffer for each of the two comm ports
unsigned long LoopCount = 0;          // unsigned 32bit, 0 to 4,294,967,295 to count times round loop
unsigned long LastCount = 0;          // loop counter the last time it was read so can calc delta
byte errorcount = 0;                  // serial receive error detected by invalid packet start/end bytes
unsigned int CommsTimeout = 0;        // used to reduce motor power if there has been no comms for a while
byte PowerScale = 7;                  // used to divide(shift) PID result changes when in low power

//
//
//                    +----+              +------+
//                  +-|PWR |--------------| USB  |-----+
//                  | |    |              |      |     |
//                  | +----+              +------+    o|
//                  |                                 o|
//                  |                                 o|AREF
//                  |                                 o|GND
//                NC|o                                o|13    TTL (software) Serial
//             IOREF|o   +----+                       o|12    TTL (software) Serial
//             RESET|o   |    |                       o|11~   PWM Motor 3
//              3.3V|o   |    |                       o|10~   PWM Motor 2
//                5V|o   |    |    Arduino            o|9~    PWM Motor 1
//               GND|o   |    |      UNO              o|8
//               GND|o   |    |                        |
//               VIN|o   |    |                       o|7      Motor 3 ENB
//                  |    |    |                       o|6~     Motor 3 ENA
//  Motor 1 Pot   A0|o   |    |                       o|5~     Motor 2 ENB
//  Motor 2 Pot   A1|o   |    |                       o|4      Motor 2 ENA
//  Motor 3 Pot   A2|o   |    |                       o|3~     Motor 1 ENB
//                A3|o   |    |                       o|2      Motor 1 ENA
//                A4|o   |    |                       o|1
//  Motion Scaler A5|o   +-/\-+                       o|0
//                  +-\                      /---------+
//                     \--------------------/
//
//

int OutputPort = PORTD;       // read the current port D bit mask
const int ENApin1 = 2;        // ENA output pin for Motor H-Bridge 1 (ie PortD bit position)
const int ENBpin1 = 3;        // ENB output pin for Motor H-Bridge 1 (ie PortD bit position)
const int ENApin2 = 4;        // ENA output pin for Motor H-Bridge 2 (ie PortD bit position)
const int ENBpin2 = 5;        // ENB output pin for Motor H-Bridge 2 (ie PortD bit position)
const int ENApin3 = 6;        // ENA output pin for Motor H-Bridge 3 (ie PortD bit position)
const int ENBpin3 = 7;        // ENB output pin for Motor H-Bridge 3 (ie PortD bit position)
const int PWMpin1 = 9;        // PWM output pin for Motor 1
const int PWMpin2 = 10;       // PWM output pin for Motor 2
const int PWMpin3 = 11;       // PWM output pin for Motor 3
const int FeedbackPin1 = A0;  // Motor 1 feedback pin
const int FeedbackPin2 = A1;  // Motor 2 feedback pin
const int FeedbackPin3 = A2;  // Motor 3 feedback pin
const int PotInputPin = A5;   // User adjustable POT used to scale the motion (if enabled)

int DeadZone1 = 0;  // feedback deadzone
int DeadZone2 = 0;  // feedback deadzone
int DeadZone3 = 0;  // feedback deadzone

int CenterOffset1 = 0;  // Adjust center offset of feedback position
int CenterOffset2 = 0;  // Adjust center offset of feedback position
int CenterOffset3 = 0;  // Adjust center offset of feedback position

int CutoffLimitMax1 = 1000;  // The position beyond which the motors are disabled
int CutoffLimitMax2 = 1000;  // The position beyond which the motors are disabled
int CutoffLimitMax3 = 1000;  // The position beyond which the motors are disabled
int CutoffLimitMin1 = 23;    // The position beyond which the motors are disabled
int CutoffLimitMin2 = 23;    // The position beyond which the motors are disabled
int CutoffLimitMin3 = 23;    // The position beyond which the motors are disabled

int InputClipMax1 = 923;  // The input position beyond which the target input is clipped
int InputClipMax2 = 923;  // The input position beyond which the target input is clipped
int InputClipMax3 = 923;  // The input position beyond which the target input is clipped
int InputClipMin1 = 100;  // The input position beyond which the target input is clipped
int InputClipMin2 = 100;  // The input position beyond which the target input is clipped
int InputClipMin3 = 100;  // The input position beyond which the target input is clipped


int LiftFactor1 = 0;  // was 10 - Increase PWM when driving motor in direction it has to work harder
int LiftFactor2 = 0;  // was 10 - Increase PWM when driving motor in direction it has to work harder

int PIDProcessDivider = 1;  // divider for the PID process timer
int PIDProcessCounter = 0;
int SerialFeedbackCounter = 0;
int SerialFeedbackEnabled = 0;
int SerialFeedbackPort = 0;

int Ks1 = 1;
long Kp1_x100 = 100;  //initial value
long Ki1_x100 = 40;
long Kd1_x100 = 40;
int Ks2 = 1;
long Kp2_x100 = 420;
long Ki2_x100 = 40;
long Kd2_x100 = 40;
int Ks3 = 1;
int Kp3_x100 = 420;
int Ki3_x100 = 40;
int Kd3_x100 = 40;
int PWMout1 = 0;
int PWMout2 = 0;
int PWMout3 = 0;

int Disable1 = 1;  //Motor stop flag
int Disable2 = 1;  //Motor stop flag
int Disable3 = 1;  //Motor stop flag
int PWMoffset1 = 50;
int PWMoffset2 = 50;
int PWMoffset3 = 50;
int PWMmax1 = 100;
int PWMmax2 = 100;
int PWMmax3 = 100;
int PWMrev1 = 200;
int PWMrev2 = 200;
int PWMrev3 = 200;
unsigned int Timer1FreqkHz = 25;  // PWM freq used for Motor 1 and 2
unsigned int Timer2FreqkHz = 31;  // PWM freq used for Motor 3

#ifdef SECOND_SERIAL
SoftwareSerial mySerial(12, 13);  // RX, TX
#endif

//****************************************************************************************************************
//    Setup the PWM pin frequencies
//
//****************************************************************************************************************


void setPwmFrequency(int pin, int divisor) {
  byte mode;
  if (pin == 5 || pin == 6 || pin == 9 || pin == 10) {
    switch (divisor) {
      case 1: mode = 0x01; break;
      case 8: mode = 0x02; break;
      case 64: mode = 0x03; break;
      case 256: mode = 0x04; break;
      case 1024: mode = 0x05; break;
      default: return;
    }
    if (pin == 5 || pin == 6) {
      TCCR0B = TCCR0B & 0b11111000 | mode;
    } else {
      TCCR1B = TCCR1B & 0b11111000 | mode;
    }
  } else {
    if (pin == 3 || pin == 11) {
      switch (divisor) {
        case 1: mode = 0x01; break;
        case 8: mode = 0x02; break;
        case 32: mode = 0x03; break;
        case 64: mode = 0x04; break;
        case 128: mode = 0x05; break;
        case 256: mode = 0x06; break;
        case 1024: mode = 0x7; break;
        default: return;
      }
      TCCR2B = TCCR2B & 0b11111000 | mode;
    }
  }
}

void InitialisePWMTimer1(unsigned int Freq)  // Used for pins 9 and 10
{
  uint8_t wgm = 8;  //setting the waveform generation mode to 8 - PWM Phase and Freq Correct
  TCCR1A = (TCCR1A & B11111100) | (wgm & B00000011);
  TCCR1B = (TCCR1B & B11100111) | ((wgm & B00001100) << 1);
  TCCR1B = (TCCR1B & B11111000) | 0x01;  // Set the prescaler to minimum (ie divide by 1)

  unsigned int CountTOP;

  CountTOP = (F_CPU / 2) / Freq;  // F_CPU is the oscillator frequency - Freq is the wanted PWM freq

  // Examples of CountTOP:
  //  400 = 20000Hz  -> 8MHz / Freq = CountTOP
  //  320 = 25000Hz
  //  266 = 30075Hz

  ICR1 = CountTOP;  // Set the TOP of the count for the PWM
}


void InitialisePWMTimer2(unsigned int Freq) {
  int Timer2DivideMode;

  if (Freq >= 15000) {
    Timer2DivideMode = 0x01;  // Anything above 15000 set divide by 1 which gives 31250Hz PWM freq
  } else {
    Timer2DivideMode = 0x02;  // Anything below 15000 set divide by 8 (mode 2) which gives 3906Hz PWM freq
  }
  TCCR2B = TCCR2B & 0b11111000 | Timer2DivideMode;
}


void MyPWMWrite(uint8_t pin, uint8_t val) {
#define OCR1A_MEM 0x88
#define OCR1B_MEM 0x8A

  pinMode(pin, OUTPUT);

  //casting "val" to be larger so that the final value (which is the partially
  //the result of multiplying two potentially high value int16s) will not truncate
  uint32_t tmp = val;

  if (val == 0)
    digitalWrite(pin, LOW);
  else if (val == 255)
    digitalWrite(pin, HIGH);
  else {
    uint16_t regLoc16 = 0;
    uint16_t top;

    switch (pin) {
      case 9:
        sbi(TCCR1A, COM1A1);
        regLoc16 = OCR1A_MEM;
        top = ICR1;  //Timer1_GetTop();
        break;
      case 10:
        sbi(TCCR1A, COM1B1);
        regLoc16 = OCR1B_MEM;
        top = ICR1;  //Timer1_GetTop();
        break;
    }
    tmp = (tmp * top) / 255;
    _SFR_MEM16(regLoc16) = tmp;  //(tmp*top)/255;
  }
}


void DisableMotor1();
void DisableMotor2();
void DisableMotor3();


//****************************************************************************************************************
//    Arduino setup subroutine called at startup/reset
//
//****************************************************************************************************************


void setup() {
  //Read all stored Parameters from EEPROM

  ReadEEProm();

  Serial.begin(500000);  //115200
                         // set the data rate for the SoftwareSerial port
#ifdef SECOND_SERIAL
  mySerial.begin(115200);
#endif
  OutputPort = PORTD;
  pinMode(ENApin1, OUTPUT);
  pinMode(ENBpin1, OUTPUT);
  pinMode(ENApin2, OUTPUT);
  pinMode(ENBpin2, OUTPUT);
  pinMode(ENApin3, OUTPUT);
  pinMode(ENBpin3, OUTPUT);
  pinMode(PWMpin1, OUTPUT);
  pinMode(PWMpin2, OUTPUT);
  pinMode(PWMpin3, OUTPUT);
  MyPWMWrite(PWMpin1, 0);  //analogWrite(PWMpin1, 0);
  MyPWMWrite(PWMpin2, 0);  //analogWrite(PWMpin2, 0);
  analogWrite(PWMpin3, 0);
  DisableMotor1();
  DisableMotor2();
  DisableMotor3();

  // Note that the base frequency for pins 3, 9, 10, and 11 is 31250 Hz
  //setPwmFrequency(PWMpin1, 1);     // Divider can be 1, 8, 64, 256
  //setPwmFrequency(PWMpin2, 1);     // Divider can be 1, 8, 64, 256
  //setPwmFrequency(PWMpin3, 1);     // Divider can be 1, 8, 64, 256
  InitialisePWMTimer1(Timer1FreqkHz * 1000);  // PWM Freq for Motor 1 and 2 can be set quite precisely - pass the desired freq and nearest selected
  InitialisePWMTimer2(Timer2FreqkHz * 1000);  // Motor 3 PWM Divider can only be 1, 8, 64, 256 giving Freqs 31250,3906, 488 and 122 Hz

  //CLKPR = 0x80;   // These two lines halve the processor clock
  //CLKPR = 0x01;

  // set analogue prescale to 16
  sbi(ADCSRA, ADPS2);
  cbi(ADCSRA, ADPS1);
  cbi(ADCSRA, ADPS0);
}

void WriteEEPRomWord(int address, int intvalue) {
  int low, high;
  high = intvalue / 256;
  low = intvalue - (256 * high);
  EEPROM.write(address, high);
  EEPROM.write(address + 1, low);
}

int ReadEEPRomWord(int address) {
  int low, high, returnvalue;
  high = EEPROM.read(address);
  low = EEPROM.read(address + 1);
  returnvalue = (high * 256) + low;
  return returnvalue;
}

//****************************************************************************************************************
//    Write all configurable parameters to EEPROM
//
//****************************************************************************************************************


void WriteEEProm() {
  EEPROM.write(0, 114);
  EEPROM.write(1, CutoffLimitMin1);
  EEPROM.write(2, InputClipMin1);

  EEPROM.write(5, DeadZone1);
  EEPROM.write(6, CutoffLimitMin2);
  EEPROM.write(7, InputClipMin2);

  EEPROM.write(10, DeadZone2);
  WriteEEPRomWord(11, Kp1_x100);  // **** Even though these variables are long the actual values stored are only 0 to 1000 so OK ****
  WriteEEPRomWord(13, Ki1_x100);
  WriteEEPRomWord(15, Kd1_x100);
  WriteEEPRomWord(17, Kp2_x100);
  WriteEEPRomWord(19, Ki2_x100);
  WriteEEPRomWord(21, Kd2_x100);
  EEPROM.write(23, PWMoffset1);
  EEPROM.write(24, PWMoffset2);
  EEPROM.write(25, PWMmax1);
  EEPROM.write(26, PWMmax2);
  EEPROM.write(27, PWMrev1);
  EEPROM.write(28, PWMrev2);
  EEPROM.write(29, PWMrev3);


  EEPROM.write(31, CutoffLimitMin3);
  EEPROM.write(32, InputClipMin3);

  EEPROM.write(35, DeadZone3);
  WriteEEPRomWord(36, Kp3_x100);
  WriteEEPRomWord(38, Ki3_x100);
  WriteEEPRomWord(40, Kd3_x100);
  EEPROM.write(42, PWMoffset3);
  EEPROM.write(43, PWMmax3);

  WriteEEPRomWord(44, Ks1);
  WriteEEPRomWord(46, Ks2);
  WriteEEPRomWord(48, Ks3);

  EEPROM.write(50, constrain(PIDProcessDivider, 1, 10));
  EEPROM.write(51, Timer1FreqkHz);
  EEPROM.write(52, Timer2FreqkHz);
}

//****************************************************************************************************************
//    Read all configurable parameters from the EEPROM.
//
//****************************************************************************************************************


void ReadEEProm() {
  int evalue = EEPROM.read(0);
  if (evalue != 114)  //EEProm was not set before, set default values
  {
    WriteEEProm();
    return;
  }
  CutoffLimitMin1 = EEPROM.read(1);
  InputClipMin1 = EEPROM.read(2);
  CutoffLimitMax1 = 1023 - CutoffLimitMin1;
  InputClipMax1 = 1023 - InputClipMin1;

  DeadZone1 = EEPROM.read(5);
  CutoffLimitMin2 = EEPROM.read(6);
  InputClipMin2 = EEPROM.read(7);
  CutoffLimitMax2 = 1023 - CutoffLimitMin2;
  InputClipMax2 = 1023 - InputClipMin2;

  DeadZone2 = EEPROM.read(10);
  Kp1_x100 = ReadEEPRomWord(11);
  Ki1_x100 = ReadEEPRomWord(13);
  Kd1_x100 = ReadEEPRomWord(15);
  Kp2_x100 = ReadEEPRomWord(17);
  Ki2_x100 = ReadEEPRomWord(19);
  Kd2_x100 = ReadEEPRomWord(21);
  PWMoffset1 = EEPROM.read(23);
  PWMoffset2 = EEPROM.read(24);
  PWMmax1 = EEPROM.read(25);
  PWMmax2 = EEPROM.read(26);
  PWMrev1 = EEPROM.read(27);
  PWMrev2 = EEPROM.read(28);
  PWMrev3 = EEPROM.read(29);


  CutoffLimitMin3 = EEPROM.read(31);
  InputClipMin3 = EEPROM.read(32);
  CutoffLimitMax3 = 1023 - CutoffLimitMin3;
  InputClipMax3 = 1023 - InputClipMin3;

  DeadZone3 = EEPROM.read(35);
  Kp3_x100 = ReadEEPRomWord(36);
  Ki3_x100 = ReadEEPRomWord(38);

  Kd3_x100 = ReadEEPRomWord(40);
  PWMoffset3 = EEPROM.read(42);
  PWMmax3 = EEPROM.read(43);
  Ks1 = ReadEEPRomWord(44);
  Ks2 = ReadEEPRomWord(46);
  Ks3 = ReadEEPRomWord(48);

  PIDProcessDivider = EEPROM.read(50);
  PIDProcessDivider = constrain(PIDProcessDivider, 1, 10);
  Timer1FreqkHz = EEPROM.read(51);
  Timer2FreqkHz = EEPROM.read(52);
}



//****************************************************************************************************************
//    Send two bytes via serial in response to a request for information
//
//****************************************************************************************************************

void SendTwoValues(int id, int v1, int v2, int ComPort) {
  if (ComPort == 0) {
    Serial.write(START_BYTE);
    Serial.write(id);
    Serial.write(v1);
    Serial.write(v2);
    Serial.write(END_BYTE);
  } else {
#ifdef SECOND_SERIAL
    mySerial.write(START_BYTE);
    mySerial.write(id);
    mySerial.write(v1);
    mySerial.write(v2);
    mySerial.write(END_BYTE);
#endif
  }
}


//****************************************************************************************************************
//    Send a single word value via serial in response to a request for information
//
//****************************************************************************************************************

void SendValue(int id, int value, int ComPort) {
  int low, high;

  high = value / 256;
  low = value - (high * 256);

  if (ComPort == 0) {
    Serial.write(START_BYTE);
    Serial.write(id);
    Serial.write(high);
    Serial.write(low);
    Serial.write(END_BYTE);
  } else {
#ifdef SECOND_SERIAL
    mySerial.write(START_BYTE);
    mySerial.write(id);
    mySerial.write(high);
    mySerial.write(low);
    mySerial.write(END_BYTE);
#endif
  }
}


//****************************************************************************************************************
//    Calculate how many PID calculations have been performed since last requested
//
//****************************************************************************************************************

int DeltaLoopCount() {
  unsigned long Delta;

  if ((LastCount == 0) || ((LoopCount - LastCount) > 32000)) {
    Delta = 0;
    LastCount = LoopCount;
  } else {
    Delta = LoopCount - LastCount;
    LastCount = LoopCount;
  }
  return (int)Delta;
}


//****************************************************************************************************************
//    Process the incoming serial commands from the specified comm port
//
//****************************************************************************************************************


void ParseCommand(int ComPort) {
  CommsTimeout = 0;  // reset the comms timeout counter to indicate we are getting packets

  switch (RxBuffer[0][ComPort]) {
    case 'A':
      Target1 = (RxBuffer[1][ComPort] * 256) + RxBuffer[2][ComPort];
#ifdef REVERSE_MOTOR1
      Target1 = 1023 - Target1;
#endif

#ifdef ENABLE_POT_SCALING
      if (PotInput < 25) {
        Target1 = 512;  // Center Motor1
      } else if (PotInput < 1000) {
        Target1 = int(float(Target1 - 512) * (0.0 + float(PotInput) * 0.001)) + 512;
      }
#endif

#ifdef ENABLE_NON_LINEAR_POT_SCALING
      if (PotInput < 25) {
        Target1 = 512;  // Center Motor1
      } else if (PotInput < 512) {
        Target1 = (Target1 - 512) << 1;
        if (Target1 < 0) {
          Target1 = int((-Target1 - float((double(Target1) * double(Target1))) * 0.000488) * float(PotInput * 0.001953));
          Target1 = (-Target1 >> 1) + 512;
        } else {
          Target1 = int((Target1 - float((double(Target1) * double(Target1))) * 0.000488) * float(PotInput * 0.001953));
          Target1 = (Target1 >> 1) + 512;
        }
      } else if (PotInput < 1000) {
        Target1 = (Target1 - 512) << 1;
        if (Target1 < 0) {
          Target1 = int(-Target1 - float((double(Target1) * double(Target1) * double(1024 - PotInput))) * 0.000000953674);
          Target1 = (-Target1 >> 1) + 512;
        } else {
          Target1 = int(Target1 - float((double(Target1) * double(Target1) * double(1024 - PotInput))) * 0.000000953674);
          Target1 = (Target1 >> 1) + 512;
        }
      }
#endif

      if (Target1 > InputClipMax1) {
        Target1 = InputClipMax1;
      } else if (Target1 < InputClipMin1) {
        Target1 = InputClipMin1;
      }
      break;
    case 'B':
      Target2 = (RxBuffer[1][ComPort] * 256) + RxBuffer[2][ComPort];
#ifdef ENABLE_POT_SCALING
      if (PotInput < 25) {
        Target2 = 512;  // Center Motor1
      } else if (PotInput < 1000) {
        Target2 = int(float(Target2 - 512) * (0.0 + float(PotInput) * 0.001)) + 512;
      }
#endif

#ifdef ENABLE_NON_LINEAR_POT_SCALING
      if (PotInput < 25) {
        Target2 = 512;  // Center Motor1
      } else if (PotInput < 512) {
        Target2 = (Target2 - 512) << 1;
        if (Target2 < 0) {
          Target2 = int((-Target2 - float((double(Target2) * double(Target2))) * 0.000488) * float(PotInput * 0.001953));
          Target2 = (-Target2 >> 1) + 512;
        } else {
          Target2 = int((Target2 - float((double(Target2) * double(Target2))) * 0.000488) * float(PotInput * 0.001953));
          Target2 = (Target2 >> 1) + 512;
        }
      } else if (PotInput < 1000) {
        Target2 = (Target2 - 512) << 1;
        if (Target2 < 0) {
          Target2 = int(-Target2 - float((double(Target2) * double(Target2) * double(1024 - PotInput))) * 0.000000953674);
          Target2 = (-Target2 >> 1) + 512;
        } else {
          Target2 = int(Target2 - float((double(Target2) * double(Target2) * double(1024 - PotInput))) * 0.000000953674);
          Target2 = (Target2 >> 1) + 512;
        }
      }
#endif

      if (Target2 > InputClipMax2) {
        Target2 = InputClipMax2;
      } else if (Target2 < InputClipMin2) {
        Target2 = InputClipMin2;
      }
      break;
    case 'C':
      Target3 = (RxBuffer[1][ComPort] * 256) + RxBuffer[2][ComPort];
      if (Target3 > InputClipMax3) {
        Target3 = InputClipMax3;
      } else if (Target3 < InputClipMin3) {
        Target3 = InputClipMin3;
      }
      break;
    case 'r':
      if (RxBuffer[1][ComPort] == 'd')  // rd - Read a value from the ArduinoPID - next byte identifies value to read
      {
        switch (RxBuffer[2][ComPort]) {
          case 'A':
#ifdef REVERSE_MOTOR1
            SendTwoValues('A', (1023 - Feedback1) / 4, (1023 - Target1) / 4, ComPort);
#else
            SendTwoValues('A', Feedback1 / 4, Target1 / 4, ComPort);
#endif
            break;
          case 'B':
            SendTwoValues('B', Feedback2 / 4, Target2 / 4, ComPort);
            break;
          case 'C':
            SendTwoValues('C', Feedback3 / 4, Target3 / 4, ComPort);
            break;
          case 'a':
            SendTwoValues('a', PIDProcessDivider * 16 + Disable1 + (Disable2 * 2) + (Disable3 * 4), constrain(PWMout1, 0, 255), ComPort);
            break;
          case 'b':
            SendTwoValues('b', PIDProcessDivider * 16 + Disable1 + (Disable2 * 2) + (Disable3 * 4), constrain(PWMout2, 0, 255), ComPort);
            break;
          case 'c':
            SendTwoValues('c', PIDProcessDivider * 16 + Disable1 + (Disable2 * 2) + (Disable3 * 4), constrain(PWMout3, 0, 255), ComPort);
            break;
          case 'D':
            SendValue('D', Kp1_x100, ComPort);
            break;
          case 'E':
            SendValue('E', Kp2_x100, ComPort);
            break;
          case 'F':
            SendValue('F', Kp3_x100, ComPort);
            break;
          case 'G':
            SendValue('G', Ki1_x100, ComPort);
            break;
          case 'H':
            SendValue('H', Ki2_x100, ComPort);
            break;
          case 'I':
            SendValue('I', Ki3_x100, ComPort);
            break;
          case 'J':
            SendValue('J', Kd1_x100, ComPort);
            break;
          case 'K':
            SendValue('K', Kd2_x100, ComPort);
            break;
          case 'L':
            SendValue('L', Kd3_x100, ComPort);
            break;
          case 'M':
            SendValue('M', Ks1, ComPort);
            break;
          case 'N':
            SendValue('N', Ks2, ComPort);
            break;
          case 'O':
            SendValue('O', Ks3, ComPort);
            break;
          case 'P':
            SendTwoValues('P', PWMoffset1, PWMmax1, ComPort);
            break;
          case 'Q':
            SendTwoValues('Q', PWMoffset2, PWMmax2, ComPort);
            break;
          case 'R':
            SendTwoValues('R', PWMoffset3, PWMmax3, ComPort);
            break;
          case 'S':
            SendTwoValues('S', CutoffLimitMin1, InputClipMin1, ComPort);
            break;
          case 'T':
            SendTwoValues('T', CutoffLimitMin2, InputClipMin2, ComPort);
            break;
          case 'U':
            SendTwoValues('U', CutoffLimitMin3, InputClipMin3, ComPort);
            break;
          case 'V':
            SendTwoValues('V', DeadZone1, PWMrev1, ComPort);
            break;
          case 'W':
            SendTwoValues('W', DeadZone2, PWMrev2, ComPort);
            break;
          case 'X':
            SendTwoValues('X', DeadZone3, PWMrev3, ComPort);
            break;
          case 'Y':
            SendTwoValues('Y', PIDProcessDivider * 16 + Disable1 + (Disable2 * 2) + (Disable3 * 4), 0, ComPort);  // Second byte not yet used
            break;
          case 'Z':
            SendValue('Z', DeltaLoopCount(), ComPort);
            //                        SendValue('Z',errorcount,ComPort);            // *** TEMP CODE FOR ETESTING
            break;
          case '~':
            SendTwoValues('~', Timer1FreqkHz, Timer2FreqkHz, ComPort);  // PWM Frequencies to set
            break;
          case '?':
            break;
            //                    default:
        }
      }
      break;
    case 'D':
      Kp1_x100 = (RxBuffer[1][ComPort] * 256) + RxBuffer[2][ComPort];
      break;
    case 'E':
      Kp2_x100 = (RxBuffer[1][ComPort] * 256) + RxBuffer[2][ComPort];
      break;
    case 'F':
      Kp3_x100 = (RxBuffer[1][ComPort] * 256) + RxBuffer[2][ComPort];
      break;
    case 'G':
      Ki1_x100 = (RxBuffer[1][ComPort] * 256) + RxBuffer[2][ComPort];
      break;
    case 'H':
      Ki2_x100 = (RxBuffer[1][ComPort] * 256) + RxBuffer[2][ComPort];
      break;
    case 'I':
      Ki3_x100 = (RxBuffer[1][ComPort] * 256) + RxBuffer[2][ComPort];
      break;
    case 'J':
      Kd1_x100 = (RxBuffer[1][ComPort] * 256) + RxBuffer[2][ComPort];
      break;
    case 'K':
      Kd2_x100 = (RxBuffer[1][ComPort] * 256) + RxBuffer[2][ComPort];
      break;
    case 'L':
      Kd3_x100 = (RxBuffer[1][ComPort] * 256) + RxBuffer[2][ComPort];
      break;
    case 'M':
      Ks1 = (RxBuffer[1][ComPort] * 256) + RxBuffer[2][ComPort];
      break;
    case 'N':
      Ks2 = (RxBuffer[1][ComPort] * 256) + RxBuffer[2][ComPort];
      break;
    case 'O':
      Ks3 = (RxBuffer[1][ComPort] * 256) + RxBuffer[2][ComPort];
      break;
    case 'P':
      PWMoffset1 = RxBuffer[1][ComPort];
      PWMmax1 = RxBuffer[2][ComPort];
      break;
    case 'Q':
      PWMoffset2 = RxBuffer[1][ComPort];
      PWMmax2 = RxBuffer[2][ComPort];
      break;
    case 'R':
      PWMoffset3 = RxBuffer[1][ComPort];
      PWMmax3 = RxBuffer[2][ComPort];
      break;
    case 'S':
      CutoffLimitMin1 = RxBuffer[1][ComPort];
      CutoffLimitMax1 = 1023 - CutoffLimitMin1;
      InputClipMin1 = RxBuffer[2][ComPort];
      InputClipMax1 = 1023 - InputClipMin1;
      break;
    case 'T':
      CutoffLimitMin2 = RxBuffer[1][ComPort];
      CutoffLimitMax2 = 1023 - CutoffLimitMin2;
      InputClipMin2 = RxBuffer[2][ComPort];
      InputClipMax2 = 1023 - InputClipMin2;
      break;
    case 'U':
      CutoffLimitMin3 = RxBuffer[1][ComPort];
      CutoffLimitMax3 = 1023 - CutoffLimitMin3;
      InputClipMin3 = RxBuffer[2][ComPort];
      InputClipMax3 = 1023 - InputClipMin3;
      break;
    case 'V':
      DeadZone1 = RxBuffer[1][ComPort];
      PWMrev1 = RxBuffer[2][ComPort];
      break;
    case 'W':
      DeadZone2 = RxBuffer[1][ComPort];
      PWMrev2 = RxBuffer[2][ComPort];
      break;
    case 'X':
      DeadZone3 = RxBuffer[1][ComPort];
      PWMrev3 = RxBuffer[2][ComPort];
      break;
    case 'Z':
      PIDProcessDivider = constrain(RxBuffer[1][ComPort], 1, 10);
      // ???=RxBuffer[2][ComPort];   // second byte not yet used
      break;
    case '~':
      Timer1FreqkHz = RxBuffer[1][ComPort];
      Timer2FreqkHz = RxBuffer[2][ComPort];
      InitialisePWMTimer1(Timer1FreqkHz * 1000);
      InitialisePWMTimer2(Timer2FreqkHz * 1000);
      break;
    case 'm':
      if (RxBuffer[1][ComPort] == 'o' && RxBuffer[2][ComPort] == '1')  // Monitor motor 1 (auto sends position and feedback data to host
      {
        SerialFeedbackEnabled = (ComPort << 4) | 1;  // Motor 1
      }
      if (RxBuffer[1][ComPort] == 'o' && RxBuffer[2][ComPort] == '2')  // Monitor motor 2 (auto sends position and feedback data to host
      {
        SerialFeedbackEnabled = (ComPort << 4) | 2;  // Motor 2
      }
      if (RxBuffer[1][ComPort] == 'o' && RxBuffer[2][ComPort] == '3')  // Monitor motor 3 (auto sends position and feedback data to host
      {
        SerialFeedbackEnabled = (ComPort << 4) | 3;  // Motor 3
      }
      if (RxBuffer[1][ComPort] == 'o' && RxBuffer[2][ComPort] == '0')  // Switch off auto monitoring data feedback
      {
        SerialFeedbackEnabled = 0;
      }
      break;
    case 's':
      if (RxBuffer[1][ComPort] == 'a' && RxBuffer[2][ComPort] == 'v')  // Save all settings to EEPROM
      {
        WriteEEProm();
      }
      break;
    case 'v':
      if (RxBuffer[1][ComPort] == 'e' && RxBuffer[2][ComPort] == 'r')  // Send back the SMC3 software version
      {
        SendValue('v', 100, ComPort);  // Software Version - divide by 100 to get version - ie 101= ver1.01
      }
      break;
    case 'e':
      if (RxBuffer[1][ComPort] == 'n' && RxBuffer[2][ComPort] == 'a')  // Enable all motors
      {
        Disable1 = 0;
        Disable2 = 0;
        Disable3 = 0;
#ifdef MODE2                            // 43A "Chinese" H-Bridge \
  // Reset any overtemp shutdowns and enable the H-Bridges  - toggle pins low then leave high
        OutputPort &= ~(1 << ENBpin1);  // Unset Motor1 In 2
        OutputPort &= ~(1 << ENBpin2);  // Unset Motor2 In 2
        OutputPort &= ~(1 << ENBpin3);  // Unset Motor3 In 2
        PORTD = OutputPort;
        OutputPort |= 1 << ENBpin1;  // Set Motor1 In 2
        OutputPort |= 1 << ENBpin2;  // Set Motor2 In 2
        OutputPort |= 1 << ENBpin3;  // Set Motor3 In 2
        PORTD = OutputPort;
#endif
      } else if (RxBuffer[1][ComPort] == 'n' && RxBuffer[2][ComPort] == '1')  // Enable motor 1
      {
        Disable1 = 0;
#ifdef MODE2                            // 43A "Chinese" H-Bridge \
  // Reset any overtemp shutdowns and enable the H-Bridges  - toggle pins low then leave high
        OutputPort &= ~(1 << ENBpin1);  // Unset Motor1 In 2
        PORTD = OutputPort;
        OutputPort |= 1 << ENBpin1;  // Set Motor1 In 2
        PORTD = OutputPort;
#endif
      } else if (RxBuffer[1][ComPort] == 'n' && RxBuffer[2][ComPort] == '2')  // Enable motor 2
      {
        Disable2 = 0;
#ifdef MODE2                            // 43A "Chinese" H-Bridge \
  // Reset any overtemp shutdowns and enable the H-Bridges  - toggle pins low then leave high
        OutputPort &= ~(1 << ENBpin2);  // Unset Motor2 In 2
        PORTD = OutputPort;
        OutputPort |= 1 << ENBpin2;  // Set Motor2 In 2
        PORTD = OutputPort;
#endif
      } else if (RxBuffer[1][ComPort] == 'n' && RxBuffer[2][ComPort] == '3')  // Enable motor 3
      {
        Disable3 = 0;
#ifdef MODE2                            // 43A "Chinese" H-Bridge \
  // Reset any overtemp shutdowns and enable the H-Bridges  - toggle pins low then leave high
        OutputPort &= ~(1 << ENBpin3);  // Unset Motor3 In 2
        PORTD = OutputPort;
        OutputPort |= 1 << ENBpin3;  // Set Motor3 In 2
        PORTD = OutputPort;
#endif
      }
      break;
    case '?':
      break;
      //        default:
  }
}

//****************************************************************************************************************
//    Check the incoming serial data to see if a command has been received
//
//****************************************************************************************************************


void CheckSerial0() {
  while (Serial.available()) {
    if (BufferEnd[COM0] == -1) {
      RxByte[COM0] = Serial.read();
      if (RxByte[COM0] != START_BYTE) {
        BufferEnd[COM0] = -1;
        errorcount++;
      } else {
        BufferEnd[COM0] = 0;
      }
    } else {
      RxByte[COM0] = Serial.read();
      RxBuffer[BufferEnd[COM0]][COM0] = RxByte[COM0];
      BufferEnd[COM0]++;
      if (BufferEnd[COM0] > 3) {
        if (RxBuffer[3][COM0] == END_BYTE) {
          ParseCommand(COM0);
        } else {
          errorcount++;
        }
        BufferEnd[COM0] = -1;
      }
    }
  }
}


void CheckSerial1() {
#ifdef SECOND_SERIAL
  while (mySerial.available()) {
    if (BufferEnd[COM1] == -1) {
      RxByte[COM1] = mySerial.read();
      if (RxByte[COM1] != START_BYTE) {
        BufferEnd[COM1] = -1;
      } else {
        BufferEnd[COM1] = 0;
      }
    } else {
      RxByte[COM1] = mySerial.read();
      RxBuffer[BufferEnd[COM1]][COM1] = RxByte[COM1];
      BufferEnd[COM1]++;
      if (BufferEnd[COM1] > 3) {
        if (RxBuffer[3][COM1] == END_BYTE) {
          ParseCommand(COM1);
        } else {
          errorcount++;
        }
        BufferEnd[COM1] = -1;
      }
    }
  }
#endif
}


#ifdef MODE1  // This mode outputs a PWM and two motor Direction pins

//****************************************************************************************************************
//    Set the Motor output pins to drive the motor in required direction and speed
//
//****************************************************************************************************************


void SetOutputsMotor1() {
  if ((Feedback1 > InputClipMax1) && (PWMrev1 != 0)) {
    PWMout1 = PWMrev1;
    OutputPort &= ~(1 << ENApin1);  // Unset Motor1 In 1
    OutputPort |= 1 << ENBpin1;     // Set Motor1 In 2
    MyPWMWrite(PWMpin1, PWMrev1);
  } else if ((Feedback1 < InputClipMin1) && (PWMrev1 != 0)) {
    PWMout1 = PWMrev1;
    OutputPort |= 1 << ENApin1;     // Set Motor1 In 1
    OutputPort &= ~(1 << ENBpin1);  // Unset Motor1 In 2
    MyPWMWrite(PWMpin1, PWMrev1);
  } else if ((Target1 > (Feedback1 + DeadZone1)) || (Target1 < (Feedback1 - DeadZone1))) {
    if (PWMout1 >= 0) {
      // Drive Motor Forward
      PWMout1 += PWMoffset1;
      if (PWMout1 > (PWMmax1 + LiftFactor1)) { PWMout1 = PWMmax1 + LiftFactor1; }
      OutputPort |= 1 << ENApin1;     // Set Motor1 In 1
      OutputPort &= ~(1 << ENBpin1);  // Unset Motor1 In 2
    } else {
      // Drive Motor Backwards
      PWMout1 = abs(PWMout1);
      PWMout1 += PWMoffset1;
      if (PWMout1 > PWMmax1) { PWMout1 = PWMmax1; }
      OutputPort &= ~(1 << ENApin1);  // Unset Motor1 In 1
      OutputPort |= 1 << ENBpin1;     // Set Motor1 In 2
    }
    MyPWMWrite(PWMpin1, PWMout1);
  } else {
    // Brake Motor
    OutputPort &= ~(1 << ENApin1);  // Unset Motor1 In 1
    OutputPort &= ~(1 << ENBpin1);  // Unset Motor1 In 2
    PWMout1 = PWMoffset1;
    MyPWMWrite(PWMpin1, 0);
  }
  PORTD = OutputPort;
}


void SetOutputsMotor2() {
  if ((Feedback2 > InputClipMax2) && (PWMrev2 != 0)) {
    PWMout2 = PWMrev2;
    OutputPort &= ~(1 << ENApin2);  // Unset Motor1 In 1
    OutputPort |= 1 << ENBpin2;     // Set Motor1 In 2
    MyPWMWrite(PWMpin2, PWMrev2);
  } else if ((Feedback2 < InputClipMin2) && (PWMrev2 != 0)) {
    PWMout2 = PWMrev2;
    OutputPort |= 1 << ENApin2;     // Set Motor1 In 1
    OutputPort &= ~(1 << ENBpin2);  // Unset Motor1 In 2
    MyPWMWrite(PWMpin2, PWMrev2);
  } else if (Target2 > (Feedback2 + DeadZone2) || Target2 < (Feedback2 - DeadZone2)) {
    if (PWMout2 >= 0) {
      // Drive Motor Forward
      PWMout2 += PWMoffset2;
      if (PWMout2 > PWMmax2 + LiftFactor2) { PWMout2 = PWMmax2 + LiftFactor2; }
      OutputPort |= 1 << ENApin2;     // Set Motor2 In 1
      OutputPort &= ~(1 << ENBpin2);  // Unset Motor2 In 2
    } else {
      // Drive Motor Backwards
      PWMout2 = abs(PWMout2);
      PWMout2 += PWMoffset2;
      if (PWMout2 > PWMmax2) { PWMout2 = PWMmax2; }
      OutputPort &= ~(1 << ENApin2);  // Unset Motor2 In 1
      OutputPort |= 1 << ENBpin2;     // Set Motor2 In 2
    }
    MyPWMWrite(PWMpin2, PWMout2);
  } else {
    // Brake Motor
    OutputPort &= ~(1 << ENApin2);  // Unset Motor2 In 1
    OutputPort &= ~(1 << ENBpin2);  // Unset Motor2 In 2
    PWMout2 = PWMoffset2;
    MyPWMWrite(PWMpin2, 0);
  }
  PORTD = OutputPort;
}


void SetOutputsMotor3() {
  if ((Feedback3 > InputClipMax3) && (PWMrev3 != 0)) {
    PWMout3 = PWMrev3;
    OutputPort &= ~(1 << ENApin3);  // Unset Motor1 In 1
    OutputPort |= 1 << ENBpin3;     // Set Motor1 In 2
    analogWrite(PWMpin3, PWMrev3);
  } else if ((Feedback3 < InputClipMin3) && (PWMrev3 != 0)) {
    PWMout3 = PWMrev3;
    OutputPort |= 1 << ENApin3;     // Set Motor1 In 1
    OutputPort &= ~(1 << ENBpin3);  // Unset Motor1 In 2
    analogWrite(PWMpin3, PWMrev3);
  } else if (Target3 > (Feedback3 + DeadZone3) || Target3 < (Feedback3 - DeadZone3)) {
    if (PWMout3 >= 0) {
      // Drive Motor Forward
      PWMout3 += PWMoffset3;
      if (PWMout3 > PWMmax3) { PWMout3 = PWMmax3; }
      OutputPort |= 1 << ENApin3;     // Set Motor3 In 1
      OutputPort &= ~(1 << ENBpin3);  // Unset Motor3 In 2
    } else {
      // Drive Motor Backwards
      PWMout3 = abs(PWMout3);
      PWMout3 += PWMoffset3;
      if (PWMout3 > PWMmax3) { PWMout3 = PWMmax3; }
      OutputPort &= ~(1 << ENApin3);  // Unset Motor3 In 1
      OutputPort |= 1 << ENBpin3;     // Set Motor3 In 2
    }
    analogWrite(PWMpin3, PWMout3);
  } else {
    // Brake Motor
    OutputPort &= ~(1 << ENApin3);  // Unset Motor3 In 1
    OutputPort &= ~(1 << ENBpin3);  // Unset Motor3 In 2
    PWMout3 = PWMoffset3;
    analogWrite(PWMpin3, 0);
  }
  PORTD = OutputPort;
}

#endif


#ifdef MODE2  // This mode outputs a H-Bridge Enable, One Dir Pin and a +/-PWM

//****************************************************************************************************************
//    Set the Motor output pins to drive the motor in required direction and speed
//
//****************************************************************************************************************


void SetOutputsMotor1()  // MODE2
{
  if ((Feedback1 > InputClipMax1) && (PWMrev1 != 0)) {
    PWMout1 = PWMrev1;
    OutputPort &= ~(1 << ENApin1);  // Unset Motor1 In 1
    MyPWMWrite(PWMpin1, PWMrev1);
  } else if ((Feedback1 < InputClipMin1) && (PWMrev1 != 0)) {
    PWMout1 = PWMrev1;
    OutputPort |= 1 << ENApin1;  // Set Motor1 In 1
    MyPWMWrite(PWMpin1, 255 - PWMrev1);
  } else if ((Target1 > (Feedback1 + DeadZone1)) || (Target1 < (Feedback1 - DeadZone1))) {
    if (PWMout1 >= 0) {
      // Drive Motor Forward
      PWMout1 += PWMoffset1;
      if (PWMout1 > (PWMmax1 + LiftFactor1)) { PWMout1 = PWMmax1 + LiftFactor1; }
      OutputPort |= 1 << ENApin1;          // Set Motor1 In 1
      MyPWMWrite(PWMpin1, 255 - PWMout1);  // Motor driven when PWM=0 (unset)
    } else {
      // Drive Motor Backwards
      PWMout1 = abs(PWMout1);
      PWMout1 += PWMoffset1;
      if (PWMout1 > PWMmax1) { PWMout1 = PWMmax1; }
      OutputPort &= ~(1 << ENApin1);  // Unset Motor1 In 1
      MyPWMWrite(PWMpin1, PWMout1);   // Motor driven when PWM=1 (set)
    }
  } else {
    // Brake Motor
    OutputPort &= ~(1 << ENApin1);  // Unset Motor1 In 1
    PWMout1 = PWMoffset1;
    MyPWMWrite(PWMpin1, 0);
  }
  PORTD = OutputPort;
}


void SetOutputsMotor2()  // MODE2
{
  if ((Feedback2 > InputClipMax2) && (PWMrev2 != 0)) {
    PWMout2 = PWMrev2;
    OutputPort &= ~(1 << ENApin2);  // Unset Motor2 In 1
    MyPWMWrite(PWMpin2, PWMrev2);
  } else if ((Feedback2 < InputClipMin2) && (PWMrev2 != 0)) {
    PWMout2 = PWMrev2;
    OutputPort |= 1 << ENApin2;  // Set Motor2 In 1
    MyPWMWrite(PWMpin2, 255 - PWMrev2);
  } else if ((Target2 > (Feedback2 + DeadZone2)) || (Target2 < (Feedback2 - DeadZone2))) {
    if (PWMout2 >= 0) {
      // Drive Motor Forward
      PWMout2 += PWMoffset2;
      if (PWMout2 > PWMmax2 + LiftFactor2) { PWMout2 = PWMmax2 + LiftFactor2; }
      OutputPort |= 1 << ENApin2;  // Set Motor2 In 1
      MyPWMWrite(PWMpin2, 255 - PWMout2);
    } else {
      // Drive Motor Backwards
      PWMout2 = abs(PWMout2);
      PWMout2 += PWMoffset2;
      if (PWMout2 > PWMmax2) { PWMout2 = PWMmax2; }
      OutputPort &= ~(1 << ENApin2);  // Unset Motor2 In 1
      MyPWMWrite(PWMpin2, PWMout2);
    }
  } else {
    // Brake Motor
    OutputPort &= ~(1 << ENApin2);  // Unset Motor2 In 1
    PWMout2 = PWMoffset2;
    MyPWMWrite(PWMpin2, 0);
  }
  PORTD = OutputPort;
}


void SetOutputsMotor3()  // MODE2
{
  if ((Feedback3 > InputClipMax3) && (PWMrev3 != 0)) {
    PWMout3 = PWMrev3;
    OutputPort &= ~(1 << ENApin3);  // Unset Motor3 In 1
    analogWrite(PWMpin3, PWMrev3);
  } else if ((Feedback3 < InputClipMin3) && (PWMrev3 != 0)) {
    PWMout3 = PWMrev3;
    OutputPort |= 1 << ENApin3;  // Set Motor3 In 1
    analogWrite(PWMpin3, 255 - PWMrev3);
  } else if ((Target3 > (Feedback3 + DeadZone3)) || (Target3 < (Feedback3 - DeadZone3))) {
    if (PWMout3 >= 0) {
      // Drive Motor Forward
      PWMout3 += PWMoffset3;
      if (PWMout3 > PWMmax3) { PWMout3 = PWMmax3; }
      OutputPort |= 1 << ENApin3;  // Set Motor3 In 1
      analogWrite(PWMpin3, 255 - PWMout3);
    } else {
      // Drive Motor Backwards
      PWMout3 = abs(PWMout3);
      PWMout3 += PWMoffset3;
      if (PWMout3 > PWMmax3) { PWMout3 = PWMmax3; }
      OutputPort &= ~(1 << ENApin3);  // Unset Motor3 In 1
      analogWrite(PWMpin3, PWMout3);
    }
  } else {
    // Brake Motor
    OutputPort &= ~(1 << ENApin3);  // Unset Motor3 In 1
    PWMout3 = PWMoffset3;
    analogWrite(PWMpin3, 0);
  }
  PORTD = OutputPort;
}

#endif




//****************************************************************************************************************
//    Calculate the motor PID output
//    Based on http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
//    Re-written to eliminate the use of float calculations to significantly increase calculation speed.
//
//****************************************************************************************************************



int CalcMotor1PID(int TargetPosition, int CurrentPosition) {
  static int Error = 0;
  static long pTerm_x100 = 0;
  static long dTerm_x100 = 0;
  static long iTerm_x100 = 0;
  static int CumError = 0;
  static int LastPosition = 0;
  static int DeltaPosition = 0;
  static int KdFilterCount = 0;

  Error = TargetPosition - CurrentPosition;
  if (abs(Error) <= DeadZone1) {
    CumError = 0;
  } else {
    CumError += Error;
    CumError = constrain(CumError, -1024, 1024);
  }

  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
  iTerm_x100 = (Ki1_x100 * (long)CumError);  // was >>6

  KdFilterCount++;
  if (KdFilterCount >= Ks1)  // Apply a level of filtering to Kd parameter to help reduce motor noise
  {
    DeltaPosition = (CurrentPosition - LastPosition);
    LastPosition = CurrentPosition;
    dTerm_x100 = (Kd1_x100 * (long)DeltaPosition);  // was <<5
    KdFilterCount = 0;
  }

  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.
}

int CalcMotor2PID(int TargetPosition, int CurrentPosition) {
  static int Error = 0;
  static long pTerm_x100 = 0;
  static long dTerm_x100 = 0;
  static long iTerm_x100 = 0;
  static int CumError = 0;
  static int LastPosition = 0;
  static int DeltaPosition = 0;
  static int KdFilterCount = 0;

  Error = TargetPosition - CurrentPosition;
  if (abs(Error) <= DeadZone2) {
    CumError = 0;
  } else {
    CumError += Error;
    CumError = constrain(CumError, -1024, 1024);  // Maybe Try 512 as the limits??????
  }

  DeltaPosition = (CurrentPosition - LastPosition);
  LastPosition = CurrentPosition;


  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
  iTerm_x100 = (Ki2_x100 * (long)CumError);

  KdFilterCount++;
  if (KdFilterCount >= Ks2)  // Apply a level of filtering to Kd parameter to help reduce motor noise
  {
    DeltaPosition = (CurrentPosition - LastPosition);
    LastPosition = CurrentPosition;
    dTerm_x100 = (Kd2_x100 * (long)DeltaPosition);
    KdFilterCount = 0;
  }

  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.
}

int CalcMotor3PID(int TargetPosition, int CurrentPosition) {
  static int Error = 0;
  static long pTerm_x100 = 0;
  static long dTerm_x100 = 0;
  static long iTerm_x100 = 0;
  static int CumError = 0;
  static int LastPosition = 0;
  static int DeltaPosition = 0;
  static int KdFilterCount = 0;

  Error = TargetPosition - CurrentPosition;
  if (abs(Error) <= DeadZone3) {
    CumError = 0;
  } else {
    CumError += Error;
    CumError = constrain(CumError, -1024, 1024);  // Maybe Try 512 as the limits??????
  }

  DeltaPosition = (CurrentPosition - LastPosition);
  LastPosition = CurrentPosition;


  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
  iTerm_x100 = (Ki3_x100 * (long)CumError) >> 6;

  KdFilterCount++;
  if (KdFilterCount >= Ks3)  // Apply a level of filtering to Kd parameter to help reduce motor noise
  {
    DeltaPosition = (CurrentPosition - LastPosition);
    LastPosition = CurrentPosition;
    dTerm_x100 = (Kd3_x100 * (long)DeltaPosition) << 5;
    KdFilterCount = 0;
  }

  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.
}



//****************************************************************************************************************
//    Disable the Motors - ie put the brakes on
//    Used when motor feedback is outside allowed range to minimise damage
//
//****************************************************************************************************************

void DisableMotor1() {
  Disable1 = 1;

  OutputPort &= ~(1 << ENApin1);
  OutputPort &= ~(1 << ENBpin1);

  PORTD = OutputPort;

  MyPWMWrite(PWMpin1, 0);
}

void DisableMotor2() {
  Disable2 = 1;

  OutputPort &= ~(1 << ENApin2);
  OutputPort &= ~(1 << ENBpin2);

  PORTD = OutputPort;

  MyPWMWrite(PWMpin2, 0);
}

void DisableMotor3() {
  Disable3 = 1;

  OutputPort &= ~(1 << ENApin3);
  OutputPort &= ~(1 << ENBpin3);

  PORTD = OutputPort;

  analogWrite(PWMpin3, 0);
}


void TogglePin() {
  static int PinOut = 0;
  PinOut = 1 - PinOut;
  digitalWrite(8, PinOut);
}


//****************************************************************************************************************
//    Main Arduino Program Loop
//
//****************************************************************************************************************

void loop() {


  // Enable all motors

  Disable1 = 0;
  Disable2 = 0;
  Disable3 = 0;

#ifdef MODE2  // 43A "Chinese" H-Bridge
  // Reset any overtemp shutdowns and enable the H-Bridges  - toggle pins low then leave high
  OutputPort &= ~(1 << ENBpin1);  // Unset Motor1 In 2
  OutputPort &= ~(1 << ENBpin2);  // Unset Motor2 In 2
  OutputPort &= ~(1 << ENBpin3);  // Unset Motor3 In 2
  PORTD = OutputPort;
  OutputPort |= 1 << ENBpin1;  // Set Motor1 In 2
  OutputPort |= 1 << ENBpin2;  // Set Motor2 In 2
  OutputPort |= 1 << ENBpin3;  // Set Motor3 In 2
  PORTD = OutputPort;
#endif


  // Some temporary debug stuff
  //   Serial.write('S');
  //   Serial.write(TCCR1A);
  //   Serial.write(TCCR1B);
  //   Serial.write(OCR1AL);
  //   Serial.write(OCR1AH);






  // Initialise the PID ready timer

  TimesUp = micros();
  PIDProcessCounter = 0;

  // Main Program loop

  while (1 == 1) {

    // Wait until its time and then update PID calcs for first motor

    while ((micros() - TimesUp) < PROCESS_PERIOD_uS) { ; }
    TimesUp += PROCESS_PERIOD_uS;
    TogglePin();  // Used for testing to monitor PID timing on Oscilloscope

    PIDProcessCounter++;

    if (PIDProcessCounter >= PIDProcessDivider) {
      PIDProcessCounter = 0;

      // Check and Update Motor 1 drive

      Feedback1 = analogRead(FeedbackPin1);
      if ((Feedback1 > CutoffLimitMax1) || (Feedback1 < CutoffLimitMin1)) { DisableMotor1(); }
      PWMout1 = CalcMotor1PID(Target1, Feedback1);
      if (Disable1 == 0) {
        SetOutputsMotor1();
      } else {
        PWMout1 = 0;
      }

      // Check and Update Motor 2 drive

      Feedback2 = analogRead(FeedbackPin2);
      if ((Feedback2 > CutoffLimitMax2) || (Feedback2 < CutoffLimitMin2)) { DisableMotor2(); }
      PWMout2 = CalcMotor2PID(Target2, Feedback2);
      if (Disable2 == 0) {
        SetOutputsMotor2();
      } else {
        PWMout2 = 0;
      }

      // Check and Update Motor 3 drive

      Feedback3 = analogRead(FeedbackPin3);
      if ((Feedback3 > CutoffLimitMax3) || (Feedback3 < CutoffLimitMin3)) { DisableMotor3(); }
      PWMout3 = CalcMotor3PID(Target3, Feedback3);
      if (Disable3 == 0) {
        SetOutputsMotor3();
      } else {
        PWMout3 = 0;
      }

      LoopCount++;
    }

    // Check if there is any received serial data and process (Hardware UART)

    CheckSerial0();

    // Check if there is any received serial data and process (Software Serial)

    CheckSerial1();

    SerialFeedbackCounter++;
    if (SerialFeedbackCounter >= 80)  // every 20ms send back position, pwm and motor status updates if enabled
    {
      SerialFeedbackPort = (SerialFeedbackEnabled >> 4) & 0x01;
      if ((SerialFeedbackEnabled & 0x03) == 1)  // Monitor Motor 1
      {
#ifdef REVERSE_MOTOR1
        SendTwoValues('A', (1023 - Feedback1) / 4, (1023 - Target1) / 4, SerialFeedbackPort);
#else
        SendTwoValues('A', Feedback1 / 4, Target1 / 4, SerialFeedbackPort);
#endif
        SendTwoValues('a', PIDProcessDivider * 16 + Disable1 + (Disable2 * 2) + (Disable3 * 4), constrain(PWMout1, 0, 255), SerialFeedbackPort);
      } else if ((SerialFeedbackEnabled & 0x03) == 2)  // Monitor Motor 2
      {
        SendTwoValues('B', Feedback2 / 4, Target2 / 4, SerialFeedbackPort);
        SendTwoValues('b', PIDProcessDivider * 16 + Disable1 + (Disable2 * 2) + (Disable3 * 4), constrain(PWMout2, 0, 255), SerialFeedbackPort);
      } else if ((SerialFeedbackEnabled & 0x03) == 3)  // Monitor Motor 3
      {
        SendTwoValues('C', Feedback3 / 4, Target3 / 4, SerialFeedbackPort);
        SendTwoValues('c', PIDProcessDivider * 16 + Disable1 + (Disable2 * 2) + (Disable3 * 4), constrain(PWMout3, 0, 255), SerialFeedbackPort);
      }
      SerialFeedbackCounter = 0;

#ifdef ENABLE_POT_SCALING
      PotInput = PotInput * 0.9;
      PotInput = PotInput + (0.1 * analogRead(PotInputPin));  // Also read the User Pot Input every 20ms
#endif

#ifdef ENABLE_NON_LINEAR_POT_SCALING
      PotInput = PotInput * 0.9;
      PotInput = PotInput + (0.1 * analogRead(PotInputPin));  // Also read the User Pot Input every 20ms
#endif
    }

    CommsTimeout++;
    if (CommsTimeout >= 60000)  // 15 second timeout ie 60000 * PROCESS_PERIOD_uS
    {
      CommsTimeout = 60000;  // So the counter doesn't overflow
      PowerScale = 9;
    } else {
      PowerScale = 7;  // Number of bits to shift PID result (ie divide by 128 - approximate for /100)
    }
  }
}
12

Screenshot 2024-12-14 001154
Screenshot 2024-12-14 0011541238×738 60.6 KB

Screenshot 2024-12-14 001532
Screenshot 2024-12-14 0015321228×735 69.5 KB

these are 2 screenshots showing feedback from pins a0 and a1 whe nothing is conected to the pins a0 shows noise a1 no noise which is right

13

Hi! Welcome to the Forum!

Wouldn't be enough to move the potentiometer to A3 and change this line in the code:

const int FeedbackPin1 = A0;  // Motor 1 feedback pin

to

const int FeedbackPin1 = A3;  // Motor 1 feedback pin

?

14

thanx and i did consider this but a3 is used for motor 3 when i build that part
maybe i could use pin 4 bt would that be enough i supose i could try is that the only bit of code to change

15

im excited to try this idea

16

And did you make the changes in the sketch to reflect this change?
The sketch in your post #11 clearly states that motor 3 pot was supposed to be connected to A2:

image
image658×352 20.8 KB

17

the code was written by someone much cleaverer than me its for a motion simuylater the runs on software called sim tols i know motors 12 and 3 run off of pins 0,1 and 2 ill try changing from 0 to 3 like you sugggest

18

wooooohoooo
thanx so much it worked i did consider it before but didnt know how
now i see power of code im addicted
thanx for all you help

19

I'm happy to hear that it worked.

I hope you can fix your problem with the R4 too. After all, it's too good to hang on the wall...

Have fun! :upside_down_face:

20

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.