x-pid pour dagu t-rex

[rulio]

bonjour a tous et merci pour tout votre travail deja effectuer ;
et bien je cherche a faire fonctionner une carte de chez dagu en modifiant le programme de sirnoname

[rulio]

Pour visualiser la carte T-REX
http://www.gotronic.fr/art-carte-de-con ... -20864.htm

besoin d aide pour modifier le code identifier et supprimer le mode serial de la la prog et le remplacer par la bibliotheque de la t rex
pour le moment ma carte communique bien avec x sim mais la commande h bridge ainsi que les potar ne fonctionne pas .
a l aide !!!!!!!!!!!!!!!!!!!!!!!!!

[rulio]

Le point sur mon avancement de la modification du code alor en modifiant le code de sirnoname je parviens à faire communiquer avec X sim mais pas de retours potar,
Apparemment il faut raccorder les pot sur sda et scl et les déclarer en A4 et a5. Mais avec le code de sirnoname pas de résultat alor j ai essayé avec l X pid pour atmel 328 et la les potar fonctionne je vais tomber dingue !!!

[rulio]

Par contre plus de com avec X sim !!!!

[Wanegain]

T'as des infos sur la bibliothèque de ta carte ?

Est-ce que tu sais comment tu dois communiquer avec elle pour piloter tes moteurs ?

Si t'as un exemple de code ça pourrait être très utile pour s'y plonger dedans...

[rulio]

La bibliothèque t Rex est dispo sur le site dagu. J aimerai pouvoir te les mettre ici mais pas trouver comment publier un code ici. En fait la carte communiqué par la pwm avec la partie puissance

[rulio]

Je pense que le code le plus approprié est l X pid atmel 328 car il se charge correctement et communiqué déjà avec X sim les potar aussi. En les déclarant en A4 et A5 voilà ce que je peu dire pour le moment

[rulio]

pj-1119.pdf

(1.28 MiB) Downloaded 640 times

je joint le pdf de la doc de la T_REX

[rulio]

Code: Select all

/*
   X-Sim PID
   This program will control two motor H-Bridge with analogue feedback and serial target input value
   Target is a Arduino UNO R3 but should work on all Arduino with Atmel 328, Arduinos with an FTDI serial chip need a change to lower baudrates of 57600
   Copyright (c) 2013 Martin Wiedenbauer, particial use is only allowed with a reference link to the x-sim.de project

   Command input protocol   (always 5 bytes, beginning with 'X' character and ends with a XOR checksum)
   'X' 1 H L C            Set motor 1 position to High and Low value 0 to 1023
   'X' 2 H L C            Set motor 2 position to High and Low value 0 to 1023
   'X' 3 H L C            Set motor 1 P Proportional value to High and Low value
   'X' 4 H L C            Set motor 2 P Proportional value to High and Low value
   'X' 5 H L C            Set motor 1 I Integral value to High and Low value
   'X' 6 H L C            Set motor 2 I Integral value to High and Low value
   'X' 7 H L C            Set motor 1 D Derivative value to High and Low value
   'X' 8 H L C            Set motor 2 D Derivative value to High and Low value

   'X' 200 0 0 C         Send back over serial port both analogue feedback raw values
   'X' 201 0 0 C         Send back over serial port the current pid count
   'X' 202 0 0 C         Send back over serial port the firmware version (used for x-sim autodetection)
   'X' 203 M V C         Write EEPROM on address M (only 0 to 255 of 1024 Bytes of the EEPROM) with new value V
   'X' 204 M 0 C         Read EEPROM on memory address M (only 0 to 255 of 1024 Bytes of the EEPROM), send back over serial the value
   'X' 205 0 0 C         Clear EEPROM
   'X' 206 0 0 C         Reread the whole EEPRom and store settings into fitting variables
   'X' 207 0 0   C         Disable power on motor 1
   'X' 208 0 0   C         Disable power on motor 2
   'X' 209 0 0   C         Enable power on motor 1
   'X' 210 0 0   C         Enable power on motor 2
   'X' 211 0 0   C         Send all debug values
   
   EEPROM memory map
   00      empty eeprom detection, 111 if set, all other are indicator to set default
   01-02   minimum 1
   03-04   maximum 1
   05      dead zone 1
   06-07   minimum 2
   08-09   maximum 2
   10      dead zone 2
   11-12   P component of motor 1
   13-14   I component of motor 1
   15-16   D component of motor 1
   17-18   P component of motor 2
   19-20   I component of motor 2
   21-22   D component of motor 2
   23      pwm1 offset
   24      pwm2 offset
   25      pwm1 maximum
   26      pwm2 maximum
   27      pwm frequency divider (1,8,64)

   

   Analog Pins

   Pin A4 - input of feedback positioning from motor SCL
   Pin A5 - input of feedback positioning from motor SDA

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

*/

#include <EEPROM.h>
#include <EEPROM.h>                                    // library to access EEPROM memory
#include <IOpins.h>                                    // defines which I/O pin is used for what function

// define constants here
#define startbyte 0x0F                                 // for serial communications each datapacket must start with this byte

// define global variables here
byte mode=0;                                           // mode=0: I2C / mode=1: Radio Control / mode=2: Bluetooth / mode=3: Shutdown
int  lowbat=550;                                       // default low battery voltage is 5.5V
byte errorflag;                                        // non zero if bad data packet received
byte pwmfreq;                                          // value from 1-7
byte i2cfreq;                                          // I2C clock frequency can be 100kHz(default) or 400kHz
byte I2Caddress;                                       // I2C slave address
int lmspeed,rmspeed;                                   // left and right motor speeds -255 to +255
byte lmbrake,rmbrake;                                  // left and right brakes - non zero values enable brake
int lmcur,rmcur;                                       // left and right motor current
int lmenc,rmenc;                                       // left and right encoder values
int volts;                                             // battery voltage*10 (accurate to 1 decimal place)
int xaxis,yaxis,zaxis;                                 // X, Y, Z accelerometer readings
int deltx,delty,deltz;                                 // X, Y, Z impact readings
int magnitude;                                         // impact magnitude
byte devibrate=50;                                     // number of 2mS intervals to wait after an impact has occured before a new impact can be recognized
int sensitivity=50;                                    // minimum magnitude required to register as an impact

                 

void setup()
{
//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

//------------------------- Define function of each I/O pin -------------------------------------------------

#define lmencpin     6  //  D6 - left  motor encoder input - optional
#define rmencpin     5  //  D5 - right motor encoder input - optional

#define lmbrkpin     4  //  D4 - left  motor brake        control    pin    HIGH = Brake
#define lmdirpin     2  //  D2 - left  motor direction    control    pin    HIGH = Forward   Low = Reverse
#define lmpwmpin     3  //  D3 - left  motor pulse width  modulation pin    0 - 255          Speed and Brake
#define lmcurpin     6  //  A6 - left  motor current      monitor    pin    0 - 1023         -20A to +20A   

#define rmbrkpin     9  //  D9 - right motor brake        control    pin    HIGH = Brake
#define rmdirpin    10  // D10 - right motor direction    control    pin    HIGH = Forward   Low = Reverse
#define rmpwmpin    11  // D11 - right motor pulse width  modulation pin    0 - 255          Speed and Brake
#define rmcurpin     7  //  A7 - right motor current      monitor    pin    0 - 1023         -20A to +20A   

#define voltspin     3  //  A3 - battery voltage          1V = 33.57        30V = 1007

#define axisxpin     0  //  A0 - accelerometer X-axis
#define axisypin     1  //  A1 - accelerometer Y-axis
#define axiszpin     2  //  A2 - accelerometer Z-axis
//Firmware version info
int firmaware_version_mayor=3;
int firmware_version_minor =0;


int virtualtarget1;
int virtualtarget2;
int currentanalogue1 = 0;
int currentanalogue2 = 0;
int target1=512;
int target2=512;
int low=0;
int high=0;
unsigned long hhigh=0;
unsigned long hlow=0;
unsigned long lhigh=0;
unsigned long llow=0;
int buffer=0;
int buffercount=-1;
int commandbuffer[5]={0};
unsigned long pidcount   = 0;      // unsigned 32bit, 0 to 4,294,967,295
byte errorcount   = 0;      // serial receive error detected by checksum


// Pot feedback inputs
int FeedbackPin1         = A4;      // select the input pin for the potentiometer 1, PC0
int FeedbackPin2         = A5;      // select the input pin for the potentiometer 2, PC1
int FeedbackMax1         = 1021;      // Maximum position of pot 1 to scale, do not use 1023 because it cannot control outside the pot range
int FeedbackMin1         = 2;      // Minimum position of pot 1 to scale, do not use 0 because it cannot control outside the pot range
int FeedbackMax2         = 1021;      // Maximum position of pot 2 to scale, do not use 1023 because it cannot control outside the pot range
int FeedbackMin2         = 2;      // 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
float quarter1            = 254.75;
float quarter2            = 254.75;
float threequarter1         = 764.25;
float threequarter2         = 764.25;
}
//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
int pwm1offset               = 50;
int pwm2offset               = 50;
int pwm1maximum               = 255;
int pwm2maximum               = 255;
float pwm1divider            = 0.8039;
float pwm2divider            = 0.8039;
float pwmfloat               = 0;
int pwmfrequencydivider         = 1; //31kHz

byte debugbyte =0;            //This values are for debug purpose and can be send via
int debuginteger =0;         //the SendDebug serial 211 command to the X-Sim plugin
double debugdouble =0;   

//========================================== Choose your desired motor PWM frequency ================================================//
  //                       Note that higher frequencies increase inductive reactance and reduce maximum torque                         //
  //                               Many smaller motors will not work efficiently at higher frequencies                                 //
  //                      The default is 122Hz. This provides relatively low noise and relatively smooth torque                        //
  //                                    This setting can be changed using I2C or Bluetooth                                             //
  //                                                                                                                                   //
  //     Thanks to macegr - http://forum.arduino.cc/index.php?PHPSESSID=n1691l4esq4up52krpcb77bgm1&topic=16612.msg121031#msg121031     //
  //===================================================================================================================================//

  //TCCR2B = TCCR2B & B11111000 | B00000001; pwmfreq=1;    // set timer 2 divisor to    1 for PWM frequency of  31250.000000000 Hz
  //TCCR2B = TCCR2B & B11111000 | B00000010; pwmfreq=2;    // set timer 2 divisor to    8 for PWM frequency of   3906.250000000 Hz
  //TCCR2B = TCCR2B & B11111000 | B00000011; pwmfreq=3;    // set timer 2 divisor to   32 for PWM frequency of    976.562500000 Hz
  //TCCR2B = TCCR2B & B11111000 | B00000100; pwmfreq=4;    // set timer 2 divisor to   64 for PWM frequency of    488.281250000 Hz
  //TCCR2B = TCCR2B & B11111000 | B00000101; pwmfreq=5;    // set timer 2 divisor to  128 for PWM frequency of    244.140625000 Hz
   // TCCR2B = TCCR2B & B11111000 | B00000110; pwmfreq=6;    // set timer 2 divisor to  256 for PWM frequency of    122.070312500 Hz
  //TCCR2B = TCCR2B & B11111000 | B00000111; pwmfreq=7;    // set timer 2 divisor to 1024 for PWM frequency of     30.517578125 Hz


 
  //Serial.begin(115200);   //Uncomment this for arduino UNO without ftdi serial chip

 //all IO pins are input by default on powerup --------- configure motor control pins for output -------- pwm autoconfigures -----------

  pinMode(lmpwmpin,OUTPUT);                            // configure left  motor PWM       pin for output
  pinMode(lmdirpin,OUTPUT);                            // configure left  motor direction pin for output
  pinMode(lmbrkpin,OUTPUT);                            // configure left  motor brake     pin for output
 
  pinMode(rmpwmpin,OUTPUT);                            // configure right motor PWM       pin for output
  pinMode(rmdirpin,OUTPUT);                            // configure right motor direction pin for output
  pinMode(rmbrkpin,OUTPUT);                            // configure right motor brake     pin for output
  pwmfreq=6;

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

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

void WriteEEProm()
{
   EEPROM.write(0,111);
   WriteEEPRomWord(1,FeedbackMin1);
   WriteEEPRomWord(3,FeedbackMax1);
   EEPROM.write(5,FeedbackPotDeadZone1);
   WriteEEPRomWord(6,FeedbackMin2);
   WriteEEPRomWord(8,FeedbackMax2);
   EEPROM.write(10,FeedbackPotDeadZone2);
   WriteEEPRomWord(11,int(proportional1*10.000));
   WriteEEPRomWord(13,int(integral1*10.000));
   WriteEEPRomWord(15,int(derivative1*10.000));
   WriteEEPRomWord(17,int(proportional2*10.000));
   WriteEEPRomWord(19,int(integral2*10.000));
   WriteEEPRomWord(21,int(derivative2*10.000));
   if(pwm1offset > 180 || pwm2offset > 180 || pwm1maximum < 200 || pwm2maximum < 200)
   {
      pwm1offset=50;
      pwm2offset=50;
      pwm1maximum=255;
      pwm2maximum=255;
      pwm1divider=0.8039;
      pwm2divider=0.8039;
   }
   EEPROM.write(23,pwm1offset);
   EEPROM.write(24,pwm2offset);
   EEPROM.write(25,pwm1maximum);
   EEPROM.write(26,pwm2maximum);
   if(pwmfrequencydivider != 1 && pwmfrequencydivider != 8)
   {
      pwmfrequencydivider=1;
   }
   EEPROM.write(27,pwmfrequencydivider);
}

void ReadEEProm()
{
   int evalue = EEPROM.read(0);
   if(evalue != 111) //EEProm was not set before, set default values
   {
      WriteEEProm();
      return;
   }
   FeedbackMin1=ReadEEPRomWord(1);
   FeedbackMax1=ReadEEPRomWord(3);
   FeedbackPotDeadZone1=EEPROM.read(5);
   FeedbackMin2=ReadEEPRomWord(6);
   FeedbackMax2=ReadEEPRomWord(8);
   FeedbackPotDeadZone2=EEPROM.read(10);
   proportional1=double(ReadEEPRomWord(11))/10.000;
   integral1=double(ReadEEPRomWord(13))/10.000;
   derivative1=double(ReadEEPRomWord(15))/10.000;
   proportional2=double(ReadEEPRomWord(17))/10.000;
   integral2=double(ReadEEPRomWord(19))/10.000;
   derivative2=double(ReadEEPRomWord(21))/10.000;
   pwm1offset=EEPROM.read(23);
   pwm2offset=EEPROM.read(24);
   pwm1maximum=EEPROM.read(25);
   pwm2maximum=EEPROM.read(26);
   if(pwm1offset > 180 || pwm2offset > 180 || pwm1maximum < 200 || pwm2maximum < 200)
   {
      pwm1offset=50;
      pwm2offset=50;
      pwm1maximum=255;
      pwm2maximum=255;
      pwm1divider=0.8039;
      pwm2divider=0.8039;
      EEPROM.write(23,pwm1offset);
      EEPROM.write(24,pwm2offset);
      EEPROM.write(25,pwm1maximum);
      EEPROM.write(26,pwm2maximum);
   }
   else
   {
      pwmfloat=float(pwm1maximum-pwm1offset);
      pwm1divider=pwmfloat/255.000;
      pwmfloat=float(pwm2maximum-pwm2offset);
      pwm2divider=pwmfloat/255.000;
   }
   pwmfrequencydivider=EEPROM.read(27);
   if(pwmfrequencydivider != 1 && pwmfrequencydivider != 8)
   {
      pwmfrequencydivider=1;
      EEPROM.write(27,pwmfrequencydivider);
   }
   quarter1=float(FeedbackMax1-FeedbackMin1)/4.000;
   quarter2=float(FeedbackMax2-FeedbackMin2)/4.000;
   threequarter1=quarter1*3.000;
   threequarter2=quarter1*3.000;
   setPwmFrequency(rmpwmpin, pwmfrequencydivider);
   setPwmFrequency(lmpwmpin, pwmfrequencydivider);
}

void SendAnalogueFeedback(int analogue1, int analogue2)
{
   high=analogue1/256;
   low=analogue1-(high*256);
   Serial.write('X');
   Serial.write(200);
   Serial.write(high);
   Serial.write(low);
   high=analogue2/256;
   low=analogue2-(high*256);
   Serial.write(high);
   Serial.write(low);
}

void SendPidCount()
{
   unsigned long value=pidcount;
   hhigh=value/16777216;
   value=value-(hhigh*16777216);
   hlow=value/65536;
   value=value-(hlow*65536);
   lhigh=value/256;
   llow=value-(lhigh*256);
   Serial.write('X');
   Serial.write(201);
   Serial.write(int(hhigh));
   Serial.write(int(hlow));
   Serial.write(int(lhigh));
   Serial.write(int(llow));
   Serial.write(errorcount);
}

void SendDebugValues()
{
   //The double is transformed into a integer * 10 !!!
   int doubletransfere=int(double(debugdouble*10.000));
   Serial.write('X');
   Serial.write(211);
   Serial.write(debugbyte);
   Serial.write(HIGHBYTE(debuginteger));
   Serial.write(LOWBYTE(debuginteger));
   Serial.write(HIGHBYTE(doubletransfere));
   Serial.write(LOWBYTE(doubletransfere));
}

void SendFirmwareVersion()
{
   Serial.write('X');
   Serial.write('-');
   Serial.write('P');
   Serial.write('I');
   Serial.write('D');
   Serial.write(' ');
   Serial.write(48+firmaware_version_mayor);
   Serial.write('.');
   Serial.write(48+firmware_version_minor);
}

void EEPromToSerial(int eeprom_address)
{
   int retvalue=EEPROM.read(eeprom_address);
   Serial.write('X');
   Serial.write(204);
   Serial.write(retvalue);
}

void ClearEEProm()
{
   for(int z=0; z < 1024; z++)
   {
      EEPROM.write(z,255);
   }
}

void ParseCommand()
{
   if(commandbuffer[0]==1)         //Set motor 1 position to High and Low value 0 to 1023
   {
      target1=(commandbuffer[1]*256)+commandbuffer[2];
      disable=0;
      return;
   }
   if(commandbuffer[0]==2)         //Set motor 2 position to High and Low value 0 to 1023
   {
      target2=(commandbuffer[1]*256)+commandbuffer[2];
      disable=0;
      return;
   }

   if(commandbuffer[0]==200)      //Send both analogue feedback raw values
   {
      SendAnalogueFeedback(currentanalogue1, currentanalogue2);
      return;
   }
   if(commandbuffer[0]==201)      //Send PID count
   {
      SendPidCount();
      return;
   }
   if(commandbuffer[0]==202)      //Send Firmware Version
   {
      SendFirmwareVersion();
      return;
   }
   if(commandbuffer[0]==203)      //Write EEPROM
   {
      EEPROM.write(commandbuffer[1],uint8_t(commandbuffer[2]));
      return;
   }
   if(commandbuffer[0]==204)      //Read EEPROM
   {
      EEPromToSerial(commandbuffer[1]);
      return;
   }
   if(commandbuffer[0]==205)      //Clear EEPROM
   {
      ClearEEProm();
      return;
   }
   if(commandbuffer[0]==206)      //Reread the whole EEPRom and store settings into fitting variables
   {
      ReadEEProm();
      return;
   }
   if(commandbuffer[0]==207 || commandbuffer[0]==208)      //Disable power on both motor
   {
      analogWrite(PWMPinM1,     0);
      UnsetMotor1Inp1();
      UnsetMotor1Inp2();
      analogWrite(PWMPinM2,     0);
      UnsetMotor2Inp1();
      UnsetMotor2Inp2();
      disable=1;
      return;
   }
   if(commandbuffer[0]==209 || commandbuffer[0]==210)      //Enable power on both motor
   {
      analogWrite(PWMPinM1,     128);
      UnsetMotor1Inp1();
      UnsetMotor1Inp2();
      analogWrite(PWMPinM2,     128);
      UnsetMotor2Inp1();
      UnsetMotor2Inp2();
      disable=0;
      return;
   }
   if(commandbuffer[0]==211)      //Send all debug values
   {
      SendDebugValues();
      return;
   }
}

void FeedbackPotWorker()
{
   currentanalogue1 = analogRead(FeedbackPin1);
   currentanalogue2 = analogRead(FeedbackPin2);
   //Notice: Minimum and maximum scaling calculation is done in the PC plugin with faster float support
}

bool CheckChecksum() //Atmel chips have a comport error rate of 2%, so we need here a checksum
{
   byte checksum=0;
   for(int z=0; z < 3; z++)
   {
      byte val=commandbuffer[z];
      checksum ^= val;
   }
   if(checksum==commandbuffer[3]){return true;}
   return false;
}

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(CheckChecksum()==true){ParseCommand();}else{errorcount++;}
            buffercount=-1;
         }
      }
   }
}

void CalculateVirtualTarget()
{
   if(turn360motor1==true)
   {
      virtualtarget1=target1;
      if(currentanalogue1 > int(threequarter1) && target1 < int(quarter1)){virtualtarget1+=FeedbackMax1;}
      else{if(currentanalogue1 < int(quarter1) && target1 > int(threequarter1)){virtualtarget1=0-FeedbackMax1-target1;}}
   }
   else
   {
      virtualtarget1=target1;
   }
   if(turn360motor2==true)
   {
      virtualtarget2=target2;
      if(currentanalogue2 > int(threequarter2) && target2 < int(quarter2)){virtualtarget2+=FeedbackMax2;}
      else{if(currentanalogue2 < int(quarter2) && target2 > int(threequarter2)){virtualtarget2=0-FeedbackMax2-target2;}}
   }
   else
   {
      virtualtarget2=target2;
   }
}



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(virtualtarget1,currentanalogue1);
   OutputM2=updateMotor2Pid(virtualtarget2,currentanalogue2);
}

void Motors()
{
  digitalWrite(lmbrkpin,lmbrake>0);                     // if left brake>0 then engage electronic braking for left motor
  digitalWrite(lmdirpin,lmspeed>0);                     // if left speed>0 then left motor direction is forward else reverse
  analogWrite (lmpwmpin,abs(lmspeed));                  // set left PWM to absolute value of left speed - if brake is engaged then PWM controls braking
  if(lmbrake>0 && lmspeed==0) lmenc=0;                  // if left brake is enabled and left speed=0 then reset left encoder counter
 
  digitalWrite(rmbrkpin,rmbrake>0);                     // if right brake>0 then engage electronic braking for right motor
  digitalWrite(rmdirpin,rmspeed>0);                     // if right speed>0 then right motor direction is forward else reverse
  analogWrite (rmpwmpin,abs(rmspeed));                  // set right PWM to absolute value of right speed - if brake is engaged then PWM controls braking
  if(rmbrake>0 && rmspeed==0) rmenc=0;                  // if right brake is enabled and right speed=0 then reset right encoder counter
}

void MotorBeep(byte beeps)                             
{
  digitalWrite(lmbrkpin,0);                             // ensure breaks are off
  digitalWrite(rmbrkpin,0);
 
  for(int b=0;b<beeps;b++)                              // loop to generate multiple beeps
  {
    for(int duration=0;duration<400;duration++)         // generate 2kHz tone for 200mS
    {
      digitalWrite(lmdirpin,1);                         // drive left  motor forward
      digitalWrite(rmdirpin,1);                         // drive right motor forward
      digitalWrite(lmpwmpin,1);                         // left  motor at 100%
      digitalWrite(rmpwmpin,1);                         // right motor at 100%
      delayMicroseconds(50);                            // limit full power to 50uS
      digitalWrite(lmpwmpin,0);                         // shutdown left  motor
      digitalWrite(rmpwmpin,0);                         // shutdown right motor
      delayMicroseconds(200);                           // wait aditional 200uS to generate 2kHz tone
     
      digitalWrite(lmdirpin,0);                         // drive left  motor backward
      digitalWrite(rmdirpin,0);                         // drive right motor backward
      digitalWrite(lmpwmpin,1);                         // left  motor at 100%
      digitalWrite(rmpwmpin,1);                         // right motor at 100%
      delayMicroseconds(50);                            // limit full power to 50uS
      digitalWrite(lmpwmpin,0);                         // shutdown left  motor
      digitalWrite(rmpwmpin,0);                         // shutdown right motor
      delayMicroseconds(200);                           // wait aditional 200uS to generate 2kHz tone
    }
    delay(200);                                         // pause for 200mS (1/5th of a second) between beeps
  }
}
 



void loop()
{
   //Read all stored PID and Feedback settings
   ReadEEProm();
   //Program loop
   while (1==1) //Important hack: Use this own real time loop code without arduino framework delays
   {
      FeedbackPotWorker();
      SerialWorker();
      CalculateVirtualTarget();
      CalculatePID();
      CalculateMotorDirection();
      if(disable==0)
      {
         SetPWM();
         SetHBridgeControl();
      }
      pidcount++;
   }
}





Voila ma derniere modif du code il y a une erreur mais je ne l'es pas encore detecter !!!!!! si quelqu un a une idée

[rulio]

Vraiment personne pour m aider ??