3DOF Arduino Sabertooth 4 x 24v wheelchair motors

Ok I've worked it out the PID operates the way it should, theres no problem as far as I can tell with sampleTime and lastTime variable all works fine assigned to different motors. My problem now lies in my setpoint calculations as the data required by the PID should be a double number datatype. I have used the same calculation and only switched datatypes and now it doesnt add up to the correct setpoint. I believe I have a positive negative problem and require forum help I've spent a few hours trying to work out why it aint working but no luck so far. here is my math.

Code:

pitch = 0 - 255; // int
roll = 0 - 255; // int
heave = 0 - 255;// int

  double pitchPos = (double)pitch; 
  double pitchNeg = (double)pitch * -1.0; //  I think the problem lies here
  double rollPos = (double)roll;
  double rollNeg = (double)roll * -1.0; // and here

  FL_Motor_Setpoint = (pitchPos + rollNeg + heave) / 3.0; // then the heave dont add up
  FR_Motor_Setpoint = (pitchPos + rollPos + heave) / 3.0;
  RL_Motor_Setpoint = (pitchNeg + rollNeg + heave) / 3.0;
  RR_Motor_Setpoint = (pitchNeg + rollPos + heave) / 3.0;

This all worked fine integar based so its something wrong with my coding, any ideas?

Ignore code above, I pasted the wrong part!
Ok somewhat 6 hours later I have now converted from integar based to double datatypes. This was essential for the PID Controller library to work. Now that this part is working correctly again I have spotted another little bug with my output value to the sabertooths. No problem this shouldnt be too hard to fix but who knows could be another 6 hours , just my luck.
I'm too knackered to carry on anymore right now and work at 6am till 2pm but tomorrow afternoon / evening I see a test flight on the horizon. The sabertooths will be powered up and finally we will have controlled motion with X-Sim. How long have I waited?
I'm going to delay this menu control system that I'm building till after I get my dose of motion simulation fix from my model in all its full 3DOF maybe I should take small steps and just do 2DOF first because it may snap my model, nobody knows but I will have the cameras rolling so you all wont miss out on anything.

you are doing well, and thanks for sharing the info you have :hi:

I'd like to give some pointers on how you should code your controller:

- You need a interrupt driven input stage that takes your feedback and one that runs faster then your PID funtion, that way you can apply some filtering to the input data (for rxample FIR-filter) and it gives you constant timebase to work with. Data could be read for example 10.000 per second

-Your PID computing function runs slower than your input stage but faster than the input stage of your control data from PC, say for example 1000 per second. Don't do just a loop that takes the input and calls right after the PID function. PID needs a timebase to work. If you call PID right after the input data is taken then the timebase is just too small for PID to work

-Take the movement input from PC 1000 per second for example.

-Best thing would be if the PID terms could be set independently to each motor that is controlled, so you need those terms to be (Kpmotor1, Kimotor1, Kdmotor1) (Kpmotor2, Kimotor2, Kdmotor2) and so forth. There are always differences for each motor no matter if they look the same, the tuning of them depends on too many factors so you can't use the same tuning parameters for all of them.

jyrki my friend from Finland,
Thankyou for your intrest in my simulator project and for the advice you are giving me it my passion to have an optimal running system especially with the four motors that need to be syncronised.

I am new to Arduino but I am picking it up very quickly, my code runs just like you describe from within in the main loop. I collect datapacket from X-Sim USO STXdatapacket-within-hereETX detecting start collecting till end of packet then moving on to reading input and creating my setpoints from axis data collected from datapacket and sending this to the PID compute function which returns my output I scale this then pass on to my motors.
I limit how often the datapacket is sent within XSim and syncronise this to the maximum looptime recorded so not sure I need to do it as you describe but I will look into it.
I havnt used Interupts yet so I will have a quick look into that too.
My looptime to is approx 200ms so gives 5 updates per second to my motor driver. This is before any optimizing.
Yes all pid terms can be set indepentently for each motor and adjusted on the fly during each loop. I have it setup the way you describe.
Where you say :

I am not sure what filtering I can do or what I will need, I just read input direct from feedback pots just before the PID compute
Thanks in advance Bart

how about posting the full code not snipits.
So we can help ya better.
Cool work mate.
:highfive:

I will do soon theres just a few little bugs I gotta work out. As one part changes so does another like with datatype conversion problem, I had to mod code in alot of places. If I post it will be to confusing for many. What I may start posting is function parts that are complete and ready. Just because as it stands the code is very big at the moment 12444bytes and will take while to understand it.

interrupt is mandatory. in case of lost connection or what ever bugs on pc side will make your motor blind to see the existence of pot. you are lucky if they just break your pots. if you built joyride style cockpit prepared the eject button or they will snap your neck. keep it up. average of 4 hours a day i take 8 month to understand and debug my own code.

Hi kubing my other friend from far away,
Ok I take your advice onboard and have read about interupts and understand there functionality, pretty easy I may add.
With this I have a question, you have interupts built in your code, where are you using them?

Feedback / Input when these values change or go out of range limits => trigger an event......
Serial.read when this data has arrived in a certain packet range or if it has stopped coming in => trigger these event....

These are areas that jumps out to me most where they might be needed with what your saying, where else may they be useful?

ok guys I can finally anounce my success or atleast to an extent.
I have a complete working closed loop with PID implemented and working beautifully altho some code refining is needed to make it more efficient it works remarkably well.
I uploaded the code to the arduino and fired up the simulator model but without power to the sabertooths.
In my findings looking at the LCD readout I display input, setpoint and output, now it has only just become apparent that sensor jitter is the big problem here and affects my PID and making it indecisive so I kind of would have an unsettled platform.
I see a problem here because my pot is mounted on the fourth gear and this many gears down the line can causes mucho backlashio. This together with sensor jitter is going to give me a rocking horse effect which is unprofessional by my standards and I want nothing short of perfect.
So I have done a little more research and come across another function that can smooth sensor jitter this with an interupt sequence is making good sence to me now, but I decided I have had enough for today I have crossed a massive mile marker in my simulator one where it all looks downhill from here and I decide to celibrate with a few vodkas and relax.
Im going to start posting some code for you guys in a few minutes instead which you will find very useful especially you eaorobbie, this ones for you baby. I will give u a detailed explanation in the code too.

OK guys here is my first contribution to the community. It is an XSim USO serial data parser on the Arduino side.
I have coded it on the Mega but should work on the Uno too.
This is all my own code, please give me a thankpost and credit me for my donation and if you intent to use it for anything more than private use please ask.

Code:

/*
X-Sim USO to Arduino parser
 
please bear in mind I'm using an Arduino Mega 2560
 
you need to setup X-Sim USO to send datapacket like this :
~2~X~a01~Y~a02~Z~a03~S~a04~A~a05~H~a06~~3~
where ~2~ means STX : start of text
and ~3~ means ETX : end of text

All axis need to be setup as decimal in XSim

The rest is up to you,
but if you have any questions and if I also have the time.
I will try to help.

BartS bartez@hotmail.co.uk

*/

// PC SERIAL VARIABLES ///////////////////////////////

#define PC_SERIAL Serial // Change this for your serial port to PC

// Setup ypur baudrate here and in XSim USO
#define PC_BAUDRATE_2400          2400
#define PC_BAUDRATE_9600          9600
#define PC_BAUDRATE_19200         19200
#define PC_BAUDRATE_38400         38400
#define PC_BAUDRATE_57600         57600
#define PC_BAUDRATE_115200        115200
#define PC_BAUDRATE_128000        128000
#define PC_BAUDRATE_256000        256000
#define PC_DEFAULT_BAUDRATE       128000

#define PC_SERIAL_CONNECT         PC_SERIAL.begin( PC_DEFAULT_BAUDRATE )

// 6DOF position variables imported through serial from PC
unsigned int pitch = 0; // range 0 - 127 - 255
unsigned int roll = 0;  // range 0 - 127 - 255
unsigned int yaw = 0;   // range 0 - 127 - 255
unsigned int sway = 0;  // range 0 - 127 - 255
unsigned int surge = 0; // range 0 - 127 - 255
unsigned int heave = 0; // range 0 - 127 - 255

int serialDelay = 80; // this depends on the speed of transfer in bps microseconds NOT milliseconds

// PC SERIAL VARIABLES ///////////////////////////////


// SETUP /////////////////////////////////////////////
void setup()
  {
  // OPEN PC SERIAL CONNECTION
  PC_SERIAL_CONNECT;
  }

// SETUP /////////////////////////////////////////////
  
  
// MAIN LOOP /////////////////////////////////////////
void loop()
  {  
  // READ IN AXIS POSITIONS THROUGH PC SERIAL DATA
  GET_SERIAL_DATAPACKET();
  
  //YOUR DATA IS NOW ACCESSIBLE HERE BY THE DEFINED AXIS NAMES
  }
  
// MAIN LOOP /////////////////////////////////////////


// FUNCTIONS /////////////////////////////////////////

// GET A SINGLE MOST RECENT DATAPACKET AT TIME OF EXECUTION
void GET_SERIAL_DATAPACKET()
  {
  static byte readSerial;
  
  PC_SERIAL.flush(); // clear serial data buffer
  
  // wait here until next serial packet first characters available
  while ( ! PC_SERIAL.available() ){delayMicroseconds(serialDelay);}

  while (PC_SERIAL.available() > 0) // run while serial data available
    {
    readSerial = PC_SERIAL.read(); // read out all characters until start of datapacket
    if ( readSerial == 2 ){break;} // 2 = STX = Start of text | We have reached our marker
    // EXIT AND GO TO READING DATAPACKET
    // if not our marker keep searching by waiting for more data to come in
    while (! PC_SERIAL.available()){delayMicroseconds(serialDelay);}
    // continue to gather variable data else loop again
    }
    
  // wait here until next serial transmission
  while ( ! PC_SERIAL.available() ){delayMicroseconds(serialDelay);}

  // PARSE THE DATAPACKET AND ASSIGN EACH AXIS OR FIELD DATA TO CORRECT VARIABLE UNTIL ETX
  while (PC_SERIAL.available() > 0) // run while serial data available
    {
    readSerial = PC_SERIAL.read(); // read in all characters after start of datapacket
    if ( readSerial == 3 ){break;} // 3 = ETX = End of text | We have reached our marker
    // EXIT AND GO TO READING DATAPACKET
    // if not our marker keep searching by waiting for more data to come in
    switch (readSerial)
      {
      case 'X': // Pitch
        pitch = GET_AXIS();
        break;
      case 'Y': // Roll
        roll = GET_AXIS();
        break;
      case 'Z': // Yaw
        yaw = GET_AXIS();
        break;
      case 'S': // Sway
        sway = GET_AXIS();
        break;
      case 'A': // Surge
        surge = GET_AXIS();
        break;
      case 'H': // Heave
        heave = GET_AXIS();
        break;
      }
    // if not our marker keep searching by waiting for more data to come in
    while (! PC_SERIAL.available()){delayMicroseconds(serialDelay);}
    }
  }

  
// GET AXIS OR FIELD FROM SERIAL DATA ////////////////
unsigned int GET_AXIS() // THIS FUNCTION 3 MS FASTER THAN
  {
  // THIS FUNCTION COLLECTS UPTO 3 CONSECUTIVE NUMERICAL CHARACTERS OF DATA SENT VIA SERIAL
  // IT WILL COLLECT UPTO 3 OR AS MANY AS IT FINDS AND RETURN THEM AS A SINGLE UNSIGNED INTEGER
  static int data100, data10, data1 = '\0';
  static int data = 0; // returned result
  static byte peekSerial;
  static int readSerial;
  
  delayMicroseconds(serialDelay); // this depends on the speed of serial transfer in bps    
  peekSerial = PC_SERIAL.peek(); // check character type before reading
  
  if ( peekSerial >= 48 && peekSerial <= 57 ) // TEST FOR A NUMERIC VALUE 0-9
    {
    readSerial = PC_SERIAL.read() - '0'; // read character type
    data1 = readSerial;
    
    delayMicroseconds(serialDelay); // this depends on the speed of transfer in bps
    peekSerial = PC_SERIAL.peek(); // check character type before reading
    
    if ( peekSerial >= 48 && peekSerial <= 57 ) // TEST FOR A NUMERIC VALUE 0-9
      {
      readSerial = PC_SERIAL.read() - '0'; // read character type
      data10 = data1;
      data1 = readSerial;
      
      delayMicroseconds(serialDelay); // this depends on the speed of transfer in bps
      peekSerial = PC_SERIAL.peek(); // check character type before reading
      
      if ( peekSerial >= 48 && peekSerial <= 57 ) // TEST FOR A NUMERIC VALUE 0-9
        {
        readSerial = PC_SERIAL.read() - '0'; // read character type
        data100 = data10;
        data10 = data1;
        data1 = readSerial;
        
        data = ((100*data100) + (10*data10)) + data1;
        }
      else { data = (10*data10) + data1; }
      }
    else { data = data1; }
    }
  return data;
  }

i use 2 interrupt for this job by multitasking my controller (in order to sweep away the FTDI and MAX232). one for PID + kalman filter(not necessary) and one for usb CDC routine (complicated). Sensor read, PID and filtering task cycle occur every 0.48ms..very fast in my case.

Wow Kubing you are very knowledgeable.
I have a lot to understand still, maybe not programming wise that is a pretty easy part for me I think I can build any function I need if I know what I need. The parts I need to understand are when to run ISR how often, what filters I can use to get better results these are the things I dont know about do you have any sources on filters that would matter to this kind of project.

I would really love to see your code structure to know what kind of stuff you got going on in there and when. I am not asking you for your programming code but more of a plain english breakdown of events what occurs in your loop and your ISR.
I program in plain english first before I write any code kind of like a guide path I can follow with my coding.

Also am I right in saying this, do you use ISR only for testing conditions are true or false and then enable them to execute in your main loop or do they run big bits of code themselves.

I found this article here http://www.uchobby.com/index.php/2007/11/24/arduino-interrupts/ it is very good at describing speed and timing.

I would appreciate any pointers you may have to filter sources I need to consider for my simulator data that I can read about. I have read some about kalman filters.

my interrupt do most of the job. Programming rules from any uController books said keep ISR simple. i break the rules in my case. keep close with your oscilloscope and calculate how big math and fast it can go. push them to the limit they can't stand anymore. then back to the rules. some of uC have their own behavior. i have built inverted pendulum first to understand the concept of filtering and pid. kalman just a sweetener in my project. we don't really need them. we are not building segway here. just a homemade simple controller for hobby only.have fun. :

If you need some more info about servo loops (as that is what this is all about basicly) then I'd suggest to look into the OpenServo project http://www.openservo.com/

It is released under the MIT-license and basicly it is what you're doing, just a bit sclaled up. Not much modifications needs to be done for the code.

QUICK UPDATE : I'm doing plenty of research on timing, how long it takes me to gather data from different sources for example sensors feedback input, retrieve serial data, assigning setpoints to my program variables, pid calculations and producing the end output. Its going well but knowing what to prioritizes is the question before firing of the motor updates.
@ : jyrki : thanks for the info when I get the time I will look into it and see if any of it could be useful to me. These interrupts are starting to become more understandable now especially with keeping timing.
@ : kubing : I working on prioratizisation I belive is the key and timing, I have done a quick test I know my serial packet has to be the key interupt for all other events to occur and whatever I recieve use a filter to predict from average datapacket, if all my other data is available within the set time frame.

You all have got my thinking way out of the box from a slow main loop sequence, I'm still working out what should go in my main loop and what could be fired off from interrsupts. I feel like I am aiming for an even more accurate and faster simulator.
The arduino has surprised me with its pid calculation I have dedicted today to testing research.
1 PID Controller Calculation cycle at 84 microseconds or 0.084 milliseconds.
sabertooth updates 2000 times a second so updates are every 500 microseconds 0.5 milliseconds.
1 Serial Datapacket cycle maximum of 14 bytes min / 26 bytes max at 128000 bps | min 875 microseconds 0.875 milliseconds | max 1625 microseconds 1.625 milliseconds.

Do you guys think I am gathering the correct type of data for setting my interrupt timing routines as priority for triggering events.

NEWS FLASH : QUICK PROJECT UPDATE >>>>>
Following good advice from my two very knowledgable friend jyrki and kubing I decided to investigate the benefits that Interrupt Service Routines could have for my simulator code. At first I could not see any benefit immediatley only possible corruption in my serial data but after 5 days of heavy brain work I have devised code plan in my head that is bombproof. I read what they post 50 times or so and finally it sunk in the importance of timing of data and processing, how the PID needs a timebase to work all these little hints in their posts have been on my mind.
I am now writing code for testing so I still dont have any idea if it will work or not. In my test I will make heavy use of the ISR timer at 1ms or 2ms intervals to trigger code in my main loop, there are other sweet things I plan with ISRs but I wont mention them yet, for now I am just going to make it work in a 50ms cycle for now that should be good for 20 updates a second to my simulator.

@ Kubing : books said keep ISR simple, I agree with the book. I am only going to use ISR as a condition tester and trigger.

But seriously guys thanks for all your help, I think I would of left my code as it was without your involvement. Now I have written much more solid Timebase code, I don't expect u to write code for me just keep giving me these wonderful hints and I will investigate them further.

500-700 lines of C code considered simple to book author's..lol. as long as your job done before refresh time than everything just fine. :lol:

Yet again I am faced with a new dilema, it turns out that my LCD is the root cause of all my speed problems within my interrupt and loop sequence.
After many hours of testing and then finally removing all my simulator functions from my code I found out that actually displaying my data on the LCD was the biggest resource consumer. To display a full screen of information with some format processing has an overhead of about 60 - 100 ms to my loop. I'm working on a new solution to resolve this mater using two arduino boards, my mega for all simulator data processing then to output through serial to my uno for LCD data display.
I dont want to loose my LCD as it keeps me informed of whats going on plus it has a nifty 5 dof joystick built in but mounted on my uno it takes my serial pins, so I am going to see if theres any way I can swap the pins over to digital.
Anybody have any solution for this problem?

Your Arduino board that is essentially just a ATMega2560 has enough grunt for all sort of things if coded wisely (http://www.atmel.com/dyn/resources/prod ... oc2549.pdf) even the USB port has it's own coprosessor (another Atmel microcontroller) so it doesn't purden the main processor.

You should really take a look inside of that OpenServo project and see how the code is structured, another servo project is dspic http://members.shaw.ca/swstuff/dspic-servo.html

Make an interrupt that reads in the AD-conversion results on it's own (maybe add a digital filter)
Make another timer interrupt that paces the PID calculations
When reading the serial data use once again interrupts(lower priority than the AD/PID int)

on main loop just parse the serial commands from serial buffer, adjust new position for PID if necessary and output to LCD (make sure your LCD routines doesn't use busy loops for waiting, not sure what kind of LCD you have but looks a bit like Nokia knockoff LCD and that if I remember correctly uses SPI-> use SPI routines with intterupts...)