public static async Task Aggregator(
// blinker
FPGA.OutputSignal <bool> LED1,
// banks
FPGA.OutputSignal <bool> Bank1,
FPGA.OutputSignal <bool> Bank2,
FPGA.OutputSignal <bool> Bank5,
// UART
FPGA.InputSignal <bool> RXD,
FPGA.OutputSignal <bool> TXD
)
{
QuokkaBoard.OutputBank(Bank1);
QuokkaBoard.InputBank(Bank2);
QuokkaBoard.OutputBank(Bank5);
IsAlive.Blink(LED1);
bool internalTXD = true;
FPGA.Config.Link(internalTXD, TXD);
const int servosCount = 3;
byte[] servosData = new byte[servosCount];
Sequential servoHandler = () =>
{
uint instanceId = FPGA.Config.InstanceId();
var servoOutputPin = new FPGA.OutputSignal <bool>();
byte value = 0;
bool servoOutput = false;
byte requestValue = 0;
FPGA.Config.Link(servoOutput, servoOutputPin);
while (true)
{
requestValue = servosData[instanceId];
if (requestValue != value)
{
if (requestValue < value)
{
value--;
}
else
{
value++;
}
}
MG996R.Write(value, out servoOutput);
}
};
FPGA.Config.OnStartup(servoHandler, servosCount);
Sequential readHandler = () =>
{
byte data = 0;
byte counter = 0;
while (true)
{
data = UART.Read(115200, RXD);
if (data == 255 || counter == 255)
{
// begin of packet or overflow
counter = 0;
}
else
{
if (counter < servosCount)
{
servosData[counter] = data;
}
counter++;
}
}
};
FPGA.Config.OnStartup(readHandler);
}
public static void DeviceControl(
FPGA.OutputSignal <bool> DOUT,
FPGA.InputSignal <bool> RXD,
FPGA.OutputSignal <bool> TXD
)
{
bool internalTXD = true;
FPGA.Config.Link(internalTXD, TXD);
object commsLock = new object();
bool internalDOUT = false;
FPGA.Config.Link(internalDOUT, DOUT);
const uint baud = 115200;
FPGA.SyncStream <uint> heartBeat = new FPGA.SyncStream <uint>();
const int servosCount = 5;
byte[] servosBuff = new byte[servosCount];
Sequential servosHandler = () =>
{
FPGA.OutputSignal <bool> ServoItem = new FPGA.OutputSignal <bool>();
bool servoOut = false;
FPGA.Config.Link(servoOut, ServoItem);
while (true)
{
uint idx = FPGA.Config.InstanceId();
byte servoData = 0;
servoData = servosBuff[idx];
MG996R.Write(servoData, out servoOut);
}
};
FPGA.Config.OnStartup(servosHandler, servosCount);
// SERVOs driver
Sequential <uint> servosDataHandler = (iteration) =>
{
byte data = 0;
lock (commsLock)
{
UART.RegisteredWrite(baud, 2, out internalTXD);
for (int i = 0; i < servosCount; i++)
{
data = UART.Read(baud, RXD);
servosBuff[i] = data;
}
}
};
FPGA.Config.OnStream(heartBeat, servosDataHandler);
// LED driver
Sequential <uint> ledHandler = (iteration) =>
{
uint data = 0;
uint[] buff = new uint[64];
lock (commsLock)
{
UART.RegisteredWrite(baud, 1, out internalTXD);
for (int i = 0; i < buff.Length; i++)
{
UART.ReadUnsigned32(baud, RXD, ref data);
buff[i] = data;
}
}
WS2812B.SyncWrite(buff, 0, buff.Length, out internalDOUT);
};
FPGA.Config.OnStream(heartBeat, ledHandler);
// main application driver
uint beat = 0;
Sequential heartBeatHandler = () =>
{
heartBeat.Write(beat);
beat++;
};
FPGA.Config.OnTimer(TimeSpan.FromMilliseconds(100), heartBeatHandler);
}
public static async Task Aggregator(
// blinker
FPGA.OutputSignal <bool> LED1,
// IO banks for Quokka board, not needed for another boards
FPGA.OutputSignal <bool> Bank1,
FPGA.OutputSignal <bool> Bank2,
FPGA.OutputSignal <bool> Bank5,
// UART
FPGA.InputSignal <bool> RXD,
FPGA.OutputSignal <bool> TXD
)
{
QuokkaBoard.OutputBank(Bank1);
QuokkaBoard.InputBank(Bank2);
QuokkaBoard.OutputBank(Bank5);
IsAlive.Blink(LED1);
bool internalTXD = true;
FPGA.Config.Link(internalTXD, TXD);
const int servosCount = 3;
byte[] servosData = new byte[servosCount];
Sequential servoHandler = () =>
{
uint instanceId = FPGA.Config.InstanceId();
var servoOutputPin = new FPGA.OutputSignal <bool>();
byte value = 0;
bool servoOutput = false;
byte requestValue = 0;
FPGA.Config.Link(servoOutput, servoOutputPin);
while (true)
{
requestValue = servosData[instanceId];
if (requestValue != value)
{
if (requestValue < value)
{
value--;
}
else
{
value++;
}
}
MG996R.Write(value, out servoOutput);
}
};
FPGA.Config.OnStartup(servoHandler, servosCount);
Sequential readHandler = () =>
{
byte counter = 0;
byte step = 5;
while (true)
{
while (counter < 180)
{
servosData[0] = counter;
servosData[1] = counter;
servosData[2] = counter;
counter += step;
FPGA.Runtime.Delay(TimeSpan.FromMilliseconds(100));
}
while (counter > 0)
{
servosData[0] = counter;
servosData[1] = counter;
servosData[2] = counter;
counter -= step;
FPGA.Runtime.Delay(TimeSpan.FromMilliseconds(100));
}
}
};
FPGA.Config.OnStartup(readHandler);
}
public static void LEDControl(
KeypadKeyCode keyCode,
FPGA.OutputSignal <bool> DOUT,
FPGA.OutputSignal <bool> Servo
)
{
bool internalDOUT = false;
FPGA.Config.Link(internalDOUT, DOUT);
byte servoValue = 0;
MG996R.Continuous(servoValue, Servo);
uint[] buff = new uint[64];
byte baseColor = 5;
Sequential ledHandler = () =>
{
uint color = 0;
switch (keyCode)
{
case KeypadKeyCode.STOP:
color = (uint)(baseColor << 16);
break;
case KeypadKeyCode.GO:
color = (uint)((baseColor << 8) | (baseColor << 16));
break;
case KeypadKeyCode.LOCK:
color = (uint)(baseColor << 8);
break;
case KeypadKeyCode.D2:
baseColor++;
break;
case KeypadKeyCode.D8:
baseColor--;
break;
// servo control
case KeypadKeyCode.ENT:
servoValue = 0;
break;
case KeypadKeyCode.D0:
servoValue = 60;
break;
case KeypadKeyCode.ESC:
servoValue = 120;
break;
case KeypadKeyCode.PWR:
servoValue = 180;
break;
}
for (byte i = 0; i < buff.Length; i++)
{
buff[i] = color;
}
Graphics.Draw(buff, out internalDOUT);
};
const bool trigger = true;
FPGA.Config.OnSignal(trigger, ledHandler);
}
public static async Task Aggregator(
// blinker
FPGA.OutputSignal <bool> LED1,
// IO banks for Quokka board, not needed for another boards
FPGA.OutputSignal <bool> Bank1,
FPGA.OutputSignal <bool> Bank2,
FPGA.OutputSignal <bool> Bank5,
// ADC
FPGA.OutputSignal <bool> ADC1NCS,
FPGA.OutputSignal <bool> ADC1SLCK,
FPGA.OutputSignal <bool> ADC1DIN,
FPGA.InputSignal <bool> ADC1DOUT,
// UART
FPGA.InputSignal <bool> RXD,
FPGA.OutputSignal <bool> TXD
)
{
QuokkaBoard.OutputBank(Bank1);
QuokkaBoard.InputBank(Bank2);
QuokkaBoard.OutputBank(Bank5);
IsAlive.Blink(LED1);
bool internalTXD = true;
FPGA.Config.Link(internalTXD, TXD);
const int servosCount = 4;
byte[] servosData = new byte[servosCount]
{
90, // main panel starts lookup up
90, // main panel starts lookup up
50, // sensor panel starts at beginning of measurement cycle
50, // sensor panel starts at beginning of measurement cycle
};
object servosLock = new object();
bool autoScan = true;
Sequential servoHandler = () =>
{
uint instanceId = FPGA.Config.InstanceId();
var servoOutputPin = new FPGA.OutputSignal <bool>();
byte value = 0;
bool servoOutput = false;
byte requestValue = 0;
FPGA.Config.Link(servoOutput, servoOutputPin);
while (true)
{
requestValue = servosData[instanceId];
if (FPGA.Config.InstanceId() <= 1)
{
// large panel moves with smoothing
if (requestValue != value)
{
if (requestValue < value)
{
value--;
}
else
{
value++;
}
}
}
else
{
value = requestValue;
}
MG996R.Write(value, out servoOutput);
}
};
FPGA.Config.OnStartup(servoHandler, servosCount);
Sequential autoScanHandler = () =>
{
while (true)
{
if (!autoScan)
{
continue;
}
byte s2Delta = 5;
sbyte s3Delta = 5;
ushort maxChannel1Value = 0;
byte s2Max = 0, s3Max = 0;
// reset sensor panel to center
for (byte i = 2; i < servosCount; i++)
{
servosData[i] = 90;
}
FPGA.Runtime.Delay(TimeSpan.FromMilliseconds(50));
byte s3 = 50;
for (byte s2 = 50; s2 <= 130; s2 += s2Delta)
{
servosData[2] = s2;
while (s3 >= 50 && s3 <= 130)
{
servosData[3] = s3;
// TODO: barrier sync all servo handlers
FPGA.Runtime.Delay(TimeSpan.FromMilliseconds(50));
ushort adcChannel1Value = 0, adcChannel2Value = 0;
ADC102S021.Read(out adcChannel1Value, out adcChannel2Value, ADC1NCS, ADC1SLCK, ADC1DIN, ADC1DOUT);
if (adcChannel1Value > maxChannel1Value)
{
s2Max = s2;
s3Max = s3;
maxChannel1Value = adcChannel1Value;
byte[] buff = new byte[3];
byte tmp;
buff[0] = 255;
buff[1] = s2Max;
buff[2] = s3Max;
for (byte i = 0; i < buff.Length; i++)
{
tmp = buff[i];
UART.Write(115200, tmp, internalTXD);
}
}
s3 = (byte)(s3 + s3Delta);
}
s3Delta = (sbyte)(-s3Delta);
s3 = (byte)(s3 + s3Delta); // compensate for overshoot
}
servosData[1] = (byte)(180 - s2Max); // should be mirrored around 90 as servo is backwards
servosData[0] = s3Max; // same value
FPGA.Runtime.Delay(TimeSpan.FromMilliseconds(1000));
}
};
FPGA.Config.OnStartup(autoScanHandler);
SetServos request = new SetServos();
FPGA.Signal <bool> requestDeserialized = new FPGA.Signal <bool>();
JSON.DeserializeFromUART <SetServos>(ref request, RXD, requestDeserialized);
Sequential onRequest = () =>
{
autoScan = request.autoRun == 0 ? false : true;
servosData[0] = request.s0;
servosData[1] = request.s1;
servosData[2] = request.s2;
servosData[3] = request.s3;
};
FPGA.Config.OnSignal(requestDeserialized, onRequest);
}
