Hey all, still working on lift sync with my motor encoders. I cam up with something i thought would work, but not a thing is budging. Is it okay that my initialize and preset 0 are in the initialize function?
#include "Main.h"
void LiftControl ( void )
{
long Left_Encoder;
long Right_Encoder;
long Error;
char LeftSpeed;
char RightSpeed;
char LeftConstant;
char RightConstant;
float KP = .6;
char LeftFrontSpeedConverter;
int ErrorTolerance;
Right_Encoder = GetIntegratedMotorEncoder ( 5 ) ;
Left_Encoder = GetIntegratedMotorEncoder ( 6 ) ;
// Level Function
Error = Left_Encoder - Right_Encoder ;
ErrorTolerance = 10 ;
if ( Error > ErrorTolerance )
{
LeftSpeed = LeftConstant * KP ; // Left Slow
RightSpeed = RightConstant ; // Right
}
else if ( Error < - ErrorTolerance )
{
LeftSpeed = LeftConstant ; // Left
RightSpeed = RightConstant * KP ; // Right Slow
}
else
{
LeftSpeed = LeftConstant ; // Left
RightSpeed = RightConstant ; // Right
}
// Joystick Commands
if ( ch6u == 1 )
{
LeftConstant = -127 ;
RightConstant = 127 ;
}
else if ( ch6d == 1 )
{
LeftConstant = 127 ;
RightConstant = -127 ;
}
else
{
LeftConstant = 0 ;
RightConstant = 0 ;
}
// Set Motor Commands
LeftFrontSpeedConverter = -LeftSpeed ;
SetMotor ( 5 , RightSpeed ) ; // Right Rear
SetMotor ( 7 , RightSpeed ) ; // Right Front
SetMotor ( 6 , LeftSpeed ) ; // LeftSpeed
SetMotor ( 4 , LeftFrontSpeedConverter ) ; // Left Front
}