1. Do not share user accounts! Any account that is shared by another person will be blocked and closed. This means: we will close not only the account that is shared, but also the main account of the user who uses another person's account. We have the ability to detect account sharing, so please do not try to cheat the system. This action will take place on 04/18/2023. Read all forum rules.
    Dismiss Notice
  2. For downloading SimTools plugins you need a Download Package. Get it with virtual coins that you receive for forum activity or Buy Download Package - We have a zero Spam tolerance so read our forum rules first.

    Buy Now a Download Plan!
  3. Do not try to cheat our system and do not post an unnecessary amount of useless posts only to earn credits here. We have a zero spam tolerance policy and this will cause a ban of your user account. Otherwise we wish you a pleasant stay here! Read the forum rules
  4. We have a few rules which you need to read and accept before posting anything here! Following these rules will keep the forum clean and your stay pleasant. Do not follow these rules can lead to permanent exclusion from this website: Read the forum rules.
    Are you a company? Read our company rules

My 2DOF Kit Build

Discussion in 'DIY Motion Simulator Projects' started by Paul Shortland, May 15, 2015.

  1. noorbeast

    noorbeast VR Tassie Devil Staff Member Moderator Race Director

    Joined:
    Jul 13, 2014
    Messages:
    20,460
    Occupation:
    Innovative tech specialist for NGOs
    Location:
    St Helens, Tasmania, Australia
    Balance:
    144,596Coins
    Ratings:
    +10,741 / 52 / -2
    My Motion Simulator:
    3DOF, DC motor, JRK
    If your system has changed you may need to re-register SimTools: https://www.xsimulator.net/community/threads/how-to-register-simtools-2-0.9720/

    If you have not applied for a free SimTools license then you can do so here: https://www.xsimulator.net/request-diy-key-simtools/
  2. Paul Shortland

    Paul Shortland Active Member

    Joined:
    May 13, 2015
    Messages:
    154
    Location:
    Northampton - UK
    Balance:
    317Coins
    Ratings:
    +74 / 1 / -0
    My Motion Simulator:
    2DOF, AC motor
    Ah yes that's it, I don't have a licence right now. Time for some paperwork!
  3. Paul Shortland

    Paul Shortland Active Member

    Joined:
    May 13, 2015
    Messages:
    154
    Location:
    Northampton - UK
    Balance:
    317Coins
    Ratings:
    +74 / 1 / -0
    My Motion Simulator:
    2DOF, AC motor
    Got my licence, yay!

    Having a, what have I not done, moment.

    I have the arduino code from @Wanegain (below) and I have set up simtools (i think).

    Tools.jpg Could anyone take a look at the details and let me know what else? Do I need do something with describe? Sim is unresponsive but powers up ok so I think it's software related.

    Arduino...
    /*
    Changelog : 1.23 : Now when the simulator receives the stop frame it returns to starting position (512)
    10 bits (16 bits value / 64)

    XL~a01~CXR~a02~C
    XL<Axis1>CXR<Axis2>C
    XE00C - End
    Pin out of arduino for Sabertooth
    Pin 8 - TX data to connect to S1

    Analog Pins

    Pin A0 - input of feedback positioning from motor 1
    Pin A1 - input of feedback positioning from motor 2

    As well 5v and GND pins tapped in to feed feedback pots too.
    */

    #include <SabertoothSimplified.h>
    #include <SoftwareSerial.h>

    //Some speed test switches for testers ;)
    #define FASTADC 1 //Hack to speed up the arduino analogue read function, comment out with // to disable this hack

    // 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 LOWBYTE(v) ((unsigned char) (v)) //Read
    #define HIGHBYTE(v) ((unsigned char) (((unsigned int) (v)) >> 8))
    #define BYTELOW(v) (*(((unsigned char *) (&v) + 1))) //Write
    #define BYTEHIGH(v) (*((unsigned char *) (&v)))

    #define GUARD_MOTOR_1_GAIN 100.0
    #define GUARD_MOTOR_2_GAIN 100.0

    int DeadZone = 5; //increase this value to reduce vibrations of motors
    int ReadAnalog = 8;

    int currentanalogue1 = 0;
    int currentanalogue2 = 0;
    int target1=512;
    int target2=512;
    int buffer=0;
    int buffercount=-1;
    int commandbuffer[5]={0};

    // Pot feedback inputs
    int FeedbackPin1 = A0; // select the input pin for the potentiometer 1, PC0
    int FeedbackPin2 = A1; // select the input pin for the potentiometer 2, PC1

    //180°

    int FeedbackMax1 = 962; // Maximum position of pot 1 to scale, do not use 1023 because it cannot control outside the pot range
    int FeedbackMin1 = 62; // Minimum position of pot 1 to scale, do not use 0 because it cannot control outside the pot range
    int FeedbackMax2 = 962; // Maximum position of pot 2 to scale, do not use 1023 because it cannot control outside the pot range
    int FeedbackMin2 = 62; // Minimum position of pot 2 to scale, do not use 0 because it cannot control outside the pot range

    //150°
    /*
    int FeedbackMax1 = 888; // Maximum position of pot 1 to scale, do not use 1023 because it cannot control outside the pot range
    int FeedbackMin1 = 136; // Minimum position of pot 1 to scale, do not use 0 because it cannot control outside the pot range
    int FeedbackMax2 = 888; // Maximum position of pot 2 to scale, do not use 1023 because it cannot control outside the pot range
    int FeedbackMin2 = 136; // Minimum position of pot 2 to scale, do not use 0 because it cannot control outside the pot range
    */

    int FeedbackPotDeadZone1 = 0; // +/- of this value will not move the motor
    int FeedbackPotDeadZone2 = 0; // +/- of this value will not move the motor

    //PID variables
    int motordirection1 = 0; // motor 1 move direction 0=brake, 1=forward, 2=reverse
    int motordirection2 = 0; // motor 2 move direction 0=brake, 1=forward, 2=reverse
    int oldmotordirection1 = 0;
    int oldmotordirection2 = 0;

    double K_motor_1 = 1;
    double proportional1 = 4.200; //initial value
    double integral1 = 0.400;
    double derivative1 = 0.400;
    double K_motor_2 = 1;
    double proportional2 = 4.200;
    double integral2 = 0.400;
    double derivative2 = 0.400;

    int OutputM1 = 0;
    int OutputM2 = 0;

    double integrated_motor_1_error = 0;
    double integrated_motor_2_error = 0;
    float last_motor_1_error = 0;
    float last_motor_2_error = 0;
    int disable = 1; //Motor stop flag

    //Variables pour la liaison serie de la sabertooth
    int PowerM1 = 0; //puissance moteur 1 [-127 127]
    int PowerM2 = 0; //puissance moteur 2 [-127 127]

    SoftwareSerial SWSerial(NOT_A_PIN, 8); //Pin 8 utilisé pour communiquer avec la Sabertooth
    SabertoothSimplified ST(SWSerial); //nom de l'objet communication serie avec une pin differente de TX

    void setup()
    {
    Serial.begin(9600);
    SWSerial.begin(38400); //boutons 2, 4 et 5 OFF
    ST.motor(1, 0);
    ST.motor(2, 0);
    ST.stop();
    disable=1;

    #if FASTADC
    // set analogue prescale to 16
    sbi(ADCSRA,ADPS2) ;
    cbi(ADCSRA,ADPS1) ;
    cbi(ADCSRA,ADPS0) ;[​IMG]
    #endif
    }

    void FeedbackPotWorker()
    {
    currentanalogue1 = 0;
    currentanalogue2 = 0;

    for(int z=0;z<ReadAnalog;z++)
    {
    currentanalogue1 += analogRead(FeedbackPin1);
    currentanalogue2 += analogRead(FeedbackPin2);
    }

    currentanalogue1 /= ReadAnalog;
    currentanalogue2 /= ReadAnalog;
    }

    void SerialWorker()
    {
    while(Serial.available())
    {
    if(buffercount==-1)
    {
    buffer = Serial.read();
    if(buffer != 'X'){buffercount=-1;}else{buffercount=0;}
    }
    else
    {
    buffer = Serial.read();
    commandbuffer[buffercount]=buffer;
    buffercount++;
    if(buffercount > 3)
    {
    if(commandbuffer[3]=='C'){ParseCommand();}
    buffercount=-1;
    }
    }
    }
    }

    void ParseCommand()
    {
    if(commandbuffer[0]=='L') //Set motor 1 position to High and Low value 0 to 1023
    {
    target1=(commandbuffer[1]*256)+commandbuffer[2];
    target1=map(target1,0,1023,FeedbackMin1,FeedbackMax1);
    disable=0;
    return;
    }
    if(commandbuffer[0]=='R') //Set motor 2 position to High and Low value 0 to 1023
    {
    target2=(commandbuffer[1]*256)+commandbuffer[2];
    target2=map(target2,0,1023,FeedbackMin2,FeedbackMax2);
    disable=0;
    return;
    }
    if(commandbuffer[0]=='E') //Disable power on both motor
    {
    unsigned long start;
    unsigned long time;
    start=millis();

    target1=512;
    target2=512;

    time = millis();

    while(time < (start + 3000)) //3s
    {
    FeedbackPotWorker();
    CalculatePID();
    CalculateMotorDirection();
    SetPWM();
    time=millis();
    }

    ST.motor(1, 0);
    ST.motor(2, 0);

    ST.stop();

    disable=1;
    return;
    }
    }

    void CalculateMotorDirection()
    {
    if(target1 > (currentanalogue1 + (DeadZone + FeedbackPotDeadZone1)) || target1 < (currentanalogue1 - (DeadZone + FeedbackPotDeadZone1)))
    {
    if (OutputM1 >= 0)
    {
    motordirection1=1; // drive motor 1 forward
    }
    else
    {
    motordirection1=2; // drive motor 1 backward
    OutputM1 = abs(OutputM1);
    }
    }
    else
    {
    motordirection1=0;
    }

    if(target2 > (currentanalogue2 + DeadZone + FeedbackPotDeadZone2) || target2 < (currentanalogue2 - (DeadZone + FeedbackPotDeadZone2)))
    {
    if (OutputM2 >= 0)
    {
    motordirection2=1; // drive motor 2 forward
    }
    else
    {
    motordirection2=2; // drive motor 2 backward
    OutputM2 = abs(OutputM2);
    }
    }
    else
    {
    motordirection2=0;
    }

    OutputM1 = constrain(OutputM1, -255, 255);
    OutputM2 = constrain(OutputM2, -255, 255);
    }

    int updateMotor1Pid(int targetPosition, int currentPosition)
    {
    float error = (float)targetPosition - (float)currentPosition;
    float pTerm_motor_R = proportional1 * error;
    integrated_motor_1_error += error;
    float iTerm_motor_R = integral1 * constrain(integrated_motor_1_error, -GUARD_MOTOR_1_GAIN, GUARD_MOTOR_1_GAIN);
    float dTerm_motor_R = derivative1 * (error - last_motor_1_error);
    last_motor_1_error = error;
    return constrain(K_motor_1*(pTerm_motor_R + iTerm_motor_R + dTerm_motor_R), -255, 255);
    }

    int updateMotor2Pid(int targetPosition, int currentPosition)
    {
    float error = (float)targetPosition - (float)currentPosition;
    float pTerm_motor_L = proportional2 * error;
    integrated_motor_2_error += error;
    float iTerm_motor_L = integral2 * constrain(integrated_motor_2_error, -GUARD_MOTOR_2_GAIN, GUARD_MOTOR_2_GAIN);
    float dTerm_motor_L = derivative2 * (error - last_motor_2_error);
    last_motor_2_error = error;

    return constrain(K_motor_2*(pTerm_motor_L + iTerm_motor_L + dTerm_motor_L), -255, 255);
    }

    void CalculatePID()
    {
    OutputM1=updateMotor1Pid(target1,currentanalogue1);
    OutputM2=updateMotor2Pid(target2,currentanalogue2);
    }

    void SetPWM()
    {
    //Set hardware pwm
    //Motor 1
    if(motordirection1 != 0)
    {
    //on fait la conversion en cas positif pour PowerM1 (0 à 127)
    if (motordirection1 == 1) //forward
    {
    PowerM1 = int(OutputM1/2);
    }
    //on fait la conversion en cas negatif pour PowerM1 (-127 à 0)
    else //reverse
    {
    PowerM1 = -int(OutputM1/2);
    }
    ST.motor(1, PowerM1);
    }
    else //motor1 must stop
    {
    ST.motor(1, 0);
    }

    //Motor 2
    if(motordirection2 != 0)
    {
    if (motordirection2 == 1) //sens positif
    {
    PowerM2 = int(OutputM2/2);
    }

    else
    {
    PowerM2 = -int(OutputM2/2);
    }
    ST.motor(2, PowerM2);
    }
    else //motor2 must stop
    {
    ST.motor(2, 0);
    }
    }

    void loop()
    {
    //Program loop
    while (1==1) //Important hack: Use this own real time loop code without arduino framework delays
    {
    FeedbackPotWorker();
    SerialWorker();
    CalculatePID();
    CalculateMotorDirection();
    if(disable==0)
    {
    SetPWM();
    }
    }
    }
  4. TFOU57

    TFOU57 Member

    Joined:
    Mar 30, 2009
    Messages:
    164
    Location:
    Thionville - France
    Balance:
    5,723Coins
    Ratings:
    +16 / 1 / -0
    My Motion Simulator:
    2DOF, AC motor, Arduino
    Hello to all Arduino programmers

    Sorry to dig up old post from March 23, 2019,

    In this code snippet is
    "target1=(commandbuffer[1]*256)+commandbuffer[2];"
    "target1" is in 16 bits?

    In the second line is it a 16-bit to 12-bit transformation?

    If I replace 1023 with 4095 in the second line of "map" code...
    With "target1=map(target1,0,4095,FeedbackMin1,FeedbackMax1);" I think I get "target1" in 12 bits.

    Can you confirm it for me?


    These 2 lines are extracted from this code
    Code:
    void ParseCommand()
     {
     if(commandbuffer[0]=='L') //Set motor 1 position to High and Low value 0 to 1023
     {
     target1=(commandbuffer[1]*256)+commandbuffer[2];
     target1=map(target1,0,1023,FeedbackMin1,FeedbackMax1);
     disable=0;
     return;
     }
     if(commandbuffer[0]=='R') //Set motor 2 position to High and Low value 0 to 1023
     {
     target2=(commandbuffer[1]*256)+commandbuffer[2];
     target2=map(target2,0,1023,FeedbackMin2,FeedbackMax2);
     disable=0;
     return;
     }
     if(commandbuffer[0]=='E') //Disable power on both motor
     {
     unsigned long start;
     unsigned long time;
     start=millis();
    
     target1=512;
     target2=512;
    
     time = millis();
    
     while(time < (start + 3000)) //3s
     {
     FeedbackPotWorker();
     CalculatePID();
     CalculateMotorDirection();
     SetPWM();
     time=millis();
     }
    
     ST.motor(1, 0);
     ST.motor(2, 0);
    
     ST.stop();
    
     disable=1;
     return;
     }
     }
    Texte original en Français

    Bonjour à tous les programmateurs Arduino
    Désolé de déterrer ancien post du 23 mars 2019,

    Dans cet extrait de code est ce que
    "target1=(commandbuffer[1]*256)+commandbuffer[2];"
    "target1"est en 16 bits ?

    Dans la seconde ligne Est-ce bien une transformation de 16 bits en 12bits ?

    Si je remplace 1023 par 4095 dans la seconde ligne de code "map"
    Avec "target1=map(target1,0,4095,FeedbackMin1,FeedbackMax1);" je pense obtenir "target1" en 12 bits.

    Pouvez vous me le confirmer ?