Cos'è ElectroYou | Login Iscriviti

ElectroYou - la comunità dei professionisti del mondo elettrico

Eliminare rumore alimentazione motore PWM

Elettronica lineare e digitale: didattica ed applicazioni

Moderatori: Foto Utentecarloc, Foto UtenteDirtyDeeds, Foto UtenteIsidoroKZ, Foto Utenteg.schgor, Foto UtenteBrunoValente

0
voti

[41] Re: Eliminare rumore alimentazione motore PWM

Messaggioda Foto UtenteSalvatore129 » 22 ott 2018, 15:18

Ciao Mario, si ricordo il tuo post e sono d'accordo con cio che avevi scritto: cioè un problema di risoluzine dei valori in scala ohm oppure come tu l'hai definita "quantizzazione".

L'unica cosa che non è corretta nel tuo post, quando si sente il rumore, il motore non ruota, non ruota in un verso o nell'altro e non trova la posizione esatta, non è così.
Il motore rimane fermo, immobile, perché è talmente vicina la posizione da raggiungere (target) che il motore non ruota.

Quindi suggerimenti?
Avatar utente
Foto UtenteSalvatore129
0 2
 
Messaggi: 44
Iscritto il: 1 feb 2018, 18:18

0
voti

[42] Re: Eliminare rumore alimentazione motore PWM

Messaggioda Foto Utentemarioursino » 22 ott 2018, 15:27

Si, ti serve un'isteresi dopo la lettura del potenziometro.

Codice: Seleziona tutto
#define ISTERESI 10

errore_posizione = pos_motore - potenziometro;

if(math.abs(errore_posizione) < ISTERESI)
    non_fare_niente();
else
    muovi_il_motore();
Avatar utente
Foto Utentemarioursino
3.687 3 9 13
G.Master EY
G.Master EY
 
Messaggi: 1314
Iscritto il: 5 dic 2009, 4:32

0
voti

[43] Re: Eliminare rumore alimentazione motore PWM

Messaggioda Foto UtenteSalvatore129 » 22 ott 2018, 15:33

Sei gentilissimo
Io purtroppo non capisco nulla di programmazione.
Se ti allego lo sketch, tu potresti indicarmi dove andare a posizionare le righe dell'isteresi?
Avatar utente
Foto UtenteSalvatore129
0 2
 
Messaggi: 44
Iscritto il: 1 feb 2018, 18:18

0
voti

[44] Re: Eliminare rumore alimentazione motore PWM

Messaggioda Foto Utentemarioursino » 22 ott 2018, 15:38

Posso provarci.
Avatar utente
Foto Utentemarioursino
3.687 3 9 13
G.Master EY
G.Master EY
 
Messaggi: 1314
Iscritto il: 5 dic 2009, 4:32

0
voti

[45] Re: Eliminare rumore alimentazione motore PWM

Messaggioda Foto UtenteSalvatore129 » 22 ott 2018, 16:05

Grazie Mario, sei gentilissimo
Premessa: lo sketch è stato sviluppato per pilotare n°2 motori, quindi 2 potenziometri in ingresso.

Codice: Seleziona tutto
/*
   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)

   Pin out of arduino for H-Bridge

   Pin 10 - PWM1 - Speed for Motor 1.
   Pin  9 - PWM2 - Speed for Motor 2.
   Pin  2 - INA1 - motor 1 turn
   Pin  3 - INA2 - motor 1 turn
   Pin  4 - INB1 - motor 2 turn
   Pin  5 - INB2 - motor 2 turn

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

//Firmware version info
int firmaware_version_mayor=1;
int firmware_version_minor =4;

//360� option for flight simulators
bool turn360motor1 = false;
bool turn360motor2 = false;

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

// fixed DATA for direct port manipulation, exchange here each value if your h-Bridge is connected to another port pin
// This pinning overview is to avoid the slow pin switching of the arduino libraries
//
//                  +-\/-+
//            PC6  1|    |28  PC5 (AI 5)
//      (D 0) PD0  2|    |27  PC4 (AI 4)
//      (D 1) PD1  3|    |26  PC3 (AI 3)
//      (D 2) PD2  4|    |25  PC2 (AI 2)
// PWM+ (D 3) PD3  5|    |24  PC1 (AI 1)
//      (D 4) PD4  6|    |23  PC0 (AI 0)
//            VCC  7|    |22  GND
//            GND  8|    |21  AREF
//            PB6  9|    |20  AVCC
//            PB7 10|    |19  PB5 (D 13)
// PWM+ (D 5) PD5 11|    |18  PB4 (D 12)
// PWM+ (D 6) PD6 12|    |17  PB3 (D 11) PWM
//      (D 7) PD7 13|    |16  PB2 (D 10) PWM
//      (D 8) PB0 14|    |15  PB1 (D 9) PWM
//                  +----+
//
int portdstatus            =PORTD;    // read the current port D bit mask
int ControlPinM1Inp1      =2;         // motor 1 INP1 output, this is the arduino pin description
int ControlPinM1Inp2      =3;         // motor 1 INP2 output, this is the arduino pin description
int ControlPinM2Inp1      =4;         // motor 2 INP1 output, this is the arduino pin description
int ControlPinM2Inp2      =5;         // motor 2 INP2 output, this is the arduino pin description
int PWMPinM1            =10;      // motor 1 PWM output
int PWMPinM2            =9;         // motor 2 PWM output

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

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 setup()
{
   //Serial.begin(115200);   //Uncomment this for arduino UNO without ftdi serial chip
   Serial.begin(9600);  //Uncomment this for arduino nano, arduino with ftdi chip or arduino duemilanove
   portdstatus=PORTD;
   pinMode(ControlPinM1Inp1, OUTPUT);
   pinMode(ControlPinM1Inp2, OUTPUT);
   pinMode(ControlPinM2Inp1, OUTPUT);
   pinMode(ControlPinM2Inp2, OUTPUT);
   pinMode(PWMPinM1,        OUTPUT);
   pinMode(PWMPinM2,        OUTPUT);
   analogWrite(PWMPinM1,     0);
   analogWrite(PWMPinM2,     0);
   UnsetMotor1Inp1();
   UnsetMotor1Inp2();
   UnsetMotor2Inp1();
   UnsetMotor2Inp2();
   disable=1;
   //TCCR1B = TCCR1B & 0b11111100; //This is a hack for changing the PWM frequency to a higher value, if removed it is 490Hz
   setPwmFrequency(9, 1);
   setPwmFrequency(10, 1);
#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(9, pwmfrequencydivider);
   setPwmFrequency(10, 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;
   }
}

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

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

void SetPWM()
{
   //Calculate PWM offset and maximum
   pwmfloat=OutputM1;
   pwmfloat*=pwm1divider;
   pwmfloat+=float(pwm1offset);
   OutputM1=pwmfloat;
   if(OutputM1 > pwm1maximum){OutputM1=pwm1maximum;}
   pwmfloat=OutputM2;
   pwmfloat*=pwm2divider;
   pwmfloat+=float(pwm2offset);
   OutputM2=pwmfloat;
   if(OutputM2 > pwm2maximum){OutputM2=pwm2maximum;}

   //Set hardware PWM
   if(motordirection1 != 0)
   {
      analogWrite(PWMPinM1, int(OutputM1));
   }
   else
   {
      analogWrite(PWMPinM1, 0);
   }
   if(motordirection2 != 0)
   {
      analogWrite(PWMPinM2, int(OutputM2));
   }
   else
   {
      analogWrite(PWMPinM2, 0);
   }
}

//Direct port manipulation, change here your port code

void SetMotor1Inp1()
{
   portdstatus |= 1 << ControlPinM1Inp1;
   PORTD = portdstatus;
}

void UnsetMotor1Inp1()
{
   portdstatus &= ~(1 << ControlPinM1Inp1);
   PORTD = portdstatus;
}

void SetMotor1Inp2()
{
   portdstatus |= 1 << ControlPinM1Inp2;
   PORTD = portdstatus;
}

void UnsetMotor1Inp2()
{
   portdstatus &= ~(1 << ControlPinM1Inp2);
   PORTD = portdstatus;
}

void SetMotor2Inp1()
{
   portdstatus |= 1 << ControlPinM2Inp1;
   PORTD = portdstatus;
}

void UnsetMotor2Inp1()
{
   portdstatus &= ~(1 << ControlPinM2Inp1);
   PORTD = portdstatus;
}

void SetMotor2Inp2()
{
   portdstatus |= 1 << ControlPinM2Inp2;
   PORTD = portdstatus;
}

void UnsetMotor2Inp2()
{
   portdstatus &= ~(1 << ControlPinM2Inp2);
   PORTD = portdstatus;
}

void SetHBridgeControl() //With direct port manipulation for speedup the arduino framework!
{
   //Motor 1
   if(motordirection1 != oldmotordirection1)
   {
      if(motordirection1 != 0)
      {
         if(motordirection1 == 1)
         {
            SetMotor1Inp1();
            UnsetMotor1Inp2();
         }
         else
         {
            UnsetMotor1Inp1();
            SetMotor1Inp2();
         }
      }
      else
      {
         UnsetMotor1Inp1();
         UnsetMotor1Inp2();
      }
      oldmotordirection1=motordirection1;
   }

   //Motor 2
   if(motordirection2 != oldmotordirection2)
   {
      if(motordirection2 != 0)
      {
         if(motordirection2 == 1)
         {
            SetMotor2Inp1();
            UnsetMotor2Inp2();
         }
         else
         {
            UnsetMotor2Inp1();
            SetMotor2Inp2();
         }
      }
      else
      {
         UnsetMotor2Inp1();
         UnsetMotor2Inp2();
      }
      oldmotordirection2=motordirection2;
   }
}

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++;
   }
}
Avatar utente
Foto UtenteSalvatore129
0 2
 
Messaggi: 44
Iscritto il: 1 feb 2018, 18:18

0
voti

[46] Re: Eliminare rumore alimentazione motore PWM

Messaggioda Foto Utentemarioursino » 22 ott 2018, 16:13

Puoi provare a modificare così le due funzioni di aggiornamento del PID:

Codice: Seleziona tutto
int updateMotor1Pid(int targetPosition, int currentPosition)   
{
   float thp = 10;
   float thm = -10;
   
   float error = (float)targetPosition - (float)currentPosition;
   
   if(error <= thp && error > 0)
      error = 0;
   else if(error >= thm && error < 0)
      error = 0;
   
   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 thp = 10;
   float thm = -10;
   
   float error = (float)targetPosition - (float)currentPosition;
   
   if(error <= thp && error > 0)
      error = 0;
   else if(error >= thm && error < 0)
      error = 0;
   
   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);
}


Gioca con quel "10" se rimani troppo distante dal setpoint.
Avatar utente
Foto Utentemarioursino
3.687 3 9 13
G.Master EY
G.Master EY
 
Messaggi: 1314
Iscritto il: 5 dic 2009, 4:32

0
voti

[47] Re: Eliminare rumore alimentazione motore PWM

Messaggioda Foto UtenteSalvatore129 » 22 ott 2018, 16:30

Non ho capito.
Dallo sketch originale devo togliere delle righe ed inserire quelle che mi hai appena allegato?
Di programmazione non ne capisco nulla, il mio limite arriva alla sola programmazione dell'Atmega tramite l'IDE di Arduino :(
Avatar utente
Foto UtenteSalvatore129
0 2
 
Messaggi: 44
Iscritto il: 1 feb 2018, 18:18

0
voti

[48] Re: Eliminare rumore alimentazione motore PWM

Messaggioda Foto Utentemarioursino » 22 ott 2018, 16:35

Ti metto qui sotto l'intero testo modificato, puoi sostituirlo interamente con quello che segue.

Codice: Seleziona tutto
/*
   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)

   Pin out of arduino for H-Bridge

   Pin 10 - PWM1 - Speed for Motor 1.
   Pin  9 - PWM2 - Speed for Motor 2.
   Pin  2 - INA1 - motor 1 turn
   Pin  3 - INA2 - motor 1 turn
   Pin  4 - INB1 - motor 2 turn
   Pin  5 - INB2 - motor 2 turn

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

//Firmware version info
int firmaware_version_mayor=1;
int firmware_version_minor =4;

//360� option for flight simulators
bool turn360motor1 = false;
bool turn360motor2 = false;

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

// fixed DATA for direct port manipulation, exchange here each value if your h-Bridge is connected to another port pin
// This pinning overview is to avoid the slow pin switching of the arduino libraries
//
//                  +-\/-+
//            PC6  1|    |28  PC5 (AI 5)
//      (D 0) PD0  2|    |27  PC4 (AI 4)
//      (D 1) PD1  3|    |26  PC3 (AI 3)
//      (D 2) PD2  4|    |25  PC2 (AI 2)
// PWM+ (D 3) PD3  5|    |24  PC1 (AI 1)
//      (D 4) PD4  6|    |23  PC0 (AI 0)
//            VCC  7|    |22  GND
//            GND  8|    |21  AREF
//            PB6  9|    |20  AVCC
//            PB7 10|    |19  PB5 (D 13)
// PWM+ (D 5) PD5 11|    |18  PB4 (D 12)
// PWM+ (D 6) PD6 12|    |17  PB3 (D 11) PWM
//      (D 7) PD7 13|    |16  PB2 (D 10) PWM
//      (D 8) PB0 14|    |15  PB1 (D 9) PWM
//                  +----+
//
int portdstatus            =PORTD;    // read the current port D bit mask
int ControlPinM1Inp1      =2;         // motor 1 INP1 output, this is the arduino pin description
int ControlPinM1Inp2      =3;         // motor 1 INP2 output, this is the arduino pin description
int ControlPinM2Inp1      =4;         // motor 2 INP1 output, this is the arduino pin description
int ControlPinM2Inp2      =5;         // motor 2 INP2 output, this is the arduino pin description
int PWMPinM1            =10;      // motor 1 PWM output
int PWMPinM2            =9;         // motor 2 PWM output

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

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 setup()
{
   //Serial.begin(115200);   //Uncomment this for arduino UNO without ftdi serial chip
   Serial.begin(9600);  //Uncomment this for arduino nano, arduino with ftdi chip or arduino duemilanove
   portdstatus=PORTD;
   pinMode(ControlPinM1Inp1, OUTPUT);
   pinMode(ControlPinM1Inp2, OUTPUT);
   pinMode(ControlPinM2Inp1, OUTPUT);
   pinMode(ControlPinM2Inp2, OUTPUT);
   pinMode(PWMPinM1,        OUTPUT);
   pinMode(PWMPinM2,        OUTPUT);
   analogWrite(PWMPinM1,     0);
   analogWrite(PWMPinM2,     0);
   UnsetMotor1Inp1();
   UnsetMotor1Inp2();
   UnsetMotor2Inp1();
   UnsetMotor2Inp2();
   disable=1;
   //TCCR1B = TCCR1B & 0b11111100; //This is a hack for changing the PWM frequency to a higher value, if removed it is 490Hz
   setPwmFrequency(9, 1);
   setPwmFrequency(10, 1);
#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(9, pwmfrequencydivider);
   setPwmFrequency(10, 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;
   }
}

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

   if(virtualtarget2 > (currentanalogue2 + FeedbackPotDeadZone2) || virtualtarget2 < (currentanalogue2 - 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 thp = 10;
   float thm = -10;
   
   float error = (float)targetPosition - (float)currentPosition;
   
   if(error <= thp && error > 0)
      error = 0;
   else if(error >= thm && error < 0)
      error = 0;
   
   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 thp = 10;
   float thm = -10;
   
   float error = (float)targetPosition - (float)currentPosition;
   
   if(error <= thp && error > 0)
      error = 0;
   else if(error >= thm && error < 0)
      error = 0;
   
   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 SetPWM()
{
   //Calculate PWM offset and maximum
   pwmfloat=OutputM1;
   pwmfloat*=pwm1divider;
   pwmfloat+=float(pwm1offset);
   OutputM1=pwmfloat;
   if(OutputM1 > pwm1maximum){OutputM1=pwm1maximum;}
   pwmfloat=OutputM2;
   pwmfloat*=pwm2divider;
   pwmfloat+=float(pwm2offset);
   OutputM2=pwmfloat;
   if(OutputM2 > pwm2maximum){OutputM2=pwm2maximum;}

   //Set hardware PWM
   if(motordirection1 != 0)
   {
      analogWrite(PWMPinM1, int(OutputM1));
   }
   else
   {
      analogWrite(PWMPinM1, 0);
   }
   if(motordirection2 != 0)
   {
      analogWrite(PWMPinM2, int(OutputM2));
   }
   else
   {
      analogWrite(PWMPinM2, 0);
   }
}

//Direct port manipulation, change here your port code

void SetMotor1Inp1()
{
   portdstatus |= 1 << ControlPinM1Inp1;
   PORTD = portdstatus;
}

void UnsetMotor1Inp1()
{
   portdstatus &= ~(1 << ControlPinM1Inp1);
   PORTD = portdstatus;
}

void SetMotor1Inp2()
{
   portdstatus |= 1 << ControlPinM1Inp2;
   PORTD = portdstatus;
}

void UnsetMotor1Inp2()
{
   portdstatus &= ~(1 << ControlPinM1Inp2);
   PORTD = portdstatus;
}

void SetMotor2Inp1()
{
   portdstatus |= 1 << ControlPinM2Inp1;
   PORTD = portdstatus;
}

void UnsetMotor2Inp1()
{
   portdstatus &= ~(1 << ControlPinM2Inp1);
   PORTD = portdstatus;
}

void SetMotor2Inp2()
{
   portdstatus |= 1 << ControlPinM2Inp2;
   PORTD = portdstatus;
}

void UnsetMotor2Inp2()
{
   portdstatus &= ~(1 << ControlPinM2Inp2);
   PORTD = portdstatus;
}

void SetHBridgeControl() //With direct port manipulation for speedup the arduino framework!
{
   //Motor 1
   if(motordirection1 != oldmotordirection1)
   {
      if(motordirection1 != 0)
      {
         if(motordirection1 == 1)
         {
            SetMotor1Inp1();
            UnsetMotor1Inp2();
         }
         else
         {
            UnsetMotor1Inp1();
            SetMotor1Inp2();
         }
      }
      else
      {
         UnsetMotor1Inp1();
         UnsetMotor1Inp2();
      }
      oldmotordirection1=motordirection1;
   }

   //Motor 2
   if(motordirection2 != oldmotordirection2)
   {
      if(motordirection2 != 0)
      {
         if(motordirection2 == 1)
         {
            SetMotor2Inp1();
            UnsetMotor2Inp2();
         }
         else
         {
            UnsetMotor2Inp1();
            SetMotor2Inp2();
         }
      }
      else
      {
         UnsetMotor2Inp1();
         UnsetMotor2Inp2();
      }
      oldmotordirection2=motordirection2;
   }
}

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++;
   }
}
Avatar utente
Foto Utentemarioursino
3.687 3 9 13
G.Master EY
G.Master EY
 
Messaggi: 1314
Iscritto il: 5 dic 2009, 4:32

0
voti

[49] Re: Eliminare rumore alimentazione motore PWM

Messaggioda Foto UtenteSalvatore129 » 22 ott 2018, 17:31

Grazie Mario, credo che stasera lo proverò immediatamente, sono molto curioso di vedere se finalmente il rumore va via.

Solo questa cosa non ho capito
Gioca con quel "10" se rimani troppo distante dal setpoint.

il valore 10 e -10 è quel valore tale che mi crea una tolleranza di zona morta "deadzone"?

Domande:

1) se modifico il valore 10 a 9, devo anche modificare in maniera speculare il valore -10 portandolo a -9?
2) I valori 10 e -10, piu sono vicini allo zero e piu la tolleranza diminuisce, quindi inizio a sentire nuovamente il rumore?
Avatar utente
Foto UtenteSalvatore129
0 2
 
Messaggi: 44
Iscritto il: 1 feb 2018, 18:18

0
voti

[50] Re: Eliminare rumore alimentazione motore PWM

Messaggioda Foto Utentemarioursino » 22 ott 2018, 19:13

Salvatore129 ha scritto:il valore 10 e -10 è quel valore tale che mi crea una tolleranza di zona morta "deadzone"?


Ebbene sì

Salvatore129 ha scritto:1) se modifico il valore 10 a 9, devo anche modificare in maniera speculare il valore -10 portandolo a -9?




Salvatore129 ha scritto:2) I valori 10 e -10, piu sono vicini allo zero e piu la tolleranza diminuisce, quindi inizio a sentire nuovamente il rumore?


Avatar utente
Foto Utentemarioursino
3.687 3 9 13
G.Master EY
G.Master EY
 
Messaggi: 1314
Iscritto il: 5 dic 2009, 4:32

PrecedenteProssimo

Torna a Elettronica generale

Chi c’è in linea

Visitano il forum: Nessuno e 32 ospiti