private Servo(ArmConfig config, int i)
{
circ = config.SERVO_SPECS[i, 0];
float fMin = config.BOUNDS_RAD[i, 0];
float fMax = config.BOUNDS_RAD[i, 1];
if (fMax == fMin && fMax == 0)
{
Console.WriteLine("Servo init axis {0}, === no bounds ===", i + 1);
fMid = float.NaN;
return; // no bounds
}
fMid = ((fMax + fMin) / 2f) + PI;
if (fMin < 0)
{
fMin += 2 * PI;
}
if (fMax < 0)
{
fMax += 2 * PI;
}
max = (int)(fMax / 2f / PI * circ);
min = (int)(fMin / 2f / PI * circ) + 1;
Console.WriteLine("Servo init axis {0}, min {1}, max {2}, fMid {3}, circ {4}", i + 1, min, max, fMid, circ);
}
public Stepper(ArmConfig config, int i) : base(config, i)
{
GlobalRev = config.SERVO_SPECS[i, 3] == 1;
this.pin = Pi.Gpio[config.SERVO_SPECS[i, 1]];
this.pin.PinMode = GpioPinDriveMode.Output;
this.pin.Write(false);
this.dir = Pi.Gpio[config.SERVO_SPECS[i, 2]];
this.dir.PinMode = GpioPinDriveMode.Output;
this.dir.Write(GlobalRev);
}
public ServoDriver()
{
config = new ArmConfig();
poseCalculator = new PoseCalculator(config);
notifyWorkerEvent = new AutoResetEvent(false);
//for(int i = 0; i<6; i++)
//{
// servoProgress[i] = 1;
// servoToStep[i] = 1;
//}
Pi.Init <Unosquare.WiringPi.BootstrapWiringPi>();
servos = config.CreateSteppers();
//servos = config.CreateTestServos();
load();
}
public static Servo MakeTestServo(ArmConfig config, int i)
{
return(new Test(config, i));
}
public static Servo MakeStepper(ArmConfig config, int i)
{
return(new Stepper(config, i));
}
public Test(ArmConfig config, int i) : base(config, i)
{
}
public PoseCalculator(ArmConfig c)
{
config = c;
}
public PoseCalculatorTest()
{
config = new ArmConfig();
calc = new PoseCalculator(config);
}
