http://www.x-sim.de/software.php?lang=eng&page=simulation_training
y estoy usando este código:
- 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)
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 - INB1 - 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++;
}
}
Y no pasa nada, nunca se mueven los motores, adjunto imágenes de mi configuración y de las conexiones, de las cuales no tengo duda. pero por las dudas.
Espero me puedan ayudar.
SitemapIndex
RSS Feed