//EventLogic,
// The operands are used according to: "Trigger value = <Register> OPERATOR DataValue"
// see p.20 in manual for more information about thie
/* Note that only bits 0-3 of Register.EventControl is considered here.
* 0 Always true
* 1 = Equal
* 2 != Not equal
* 3 < Less than
* 4 > Greater than
* 5 or Bitwise or
* 6 nor Bitwise not or
* 7 and Bitwise and
* 8 nand Bitwise not and
* 9 xor Bitwise exclusive or
* 10 nxor Bitwise not exclusive or
* 11 + Add
* 12 - Subtract
* 13 * Multiply
* 14 / Divide
* 15 Value Takes data value directly
*/
//
// Function to create events. See p.19-21 in manual for more information about events
public void CreateEvent(ushort EventNr,
Int16 TrgData,
Int16 TrgReg,
ushort EventLogic,
Int16 DstRegister,
ushort SrcData,
Int16 SrcRegister)
{
/* (ushort) EventNr, Number between 0-19
* (Int16) TrgData, Value that is used for event triggers
* (Int16) TrgReg, Register that is read and used to trigger event
* (ushort) EventLogic, 0XA--B where A is how the event should behave
* and B is how the event should be triggered
* (Int16) DstRegister, Destination register
* (ushort) SrcData, What should be written to the destination register
* (Int16) SrcRegister, Combined with SrcData
*/
// Write to the registers to "implement/create" the events
ModCom.RunModbus((ushort)(Register.EventTrgData + EventNr), TrgData);
ModCom.RunModbus((ushort)(Register.EventTrgReg + EventNr), TrgReg);
ModCom.RunModbus((ushort)(Register.EventControl + EventNr), EventLogic);
ModCom.RunModbus((ushort)(Register.EventDstReg + EventNr), DstRegister);
ModCom.RunModbus((ushort)(Register.EventSrcData + EventNr), SrcData);
ModCom.RunModbus((ushort)(Register.EventSrcReg + EventNr), SrcRegister);
}
// Function for Sequence Control
public void sequenceControl(List <Int32> ticks, List <Double> times)
{
// Wrtie the first 16 targets
// Save values from the log registers
for (ushort i = 0; i < 16; i++)
{
// Variables for increasing the register numbers
ushort ch1 = (ushort)(Register.SeqTarget + i);
ushort ch2 = (ushort)(Register.SeqTime + i);
// Write values
ModCom.RunModbus(ch1, ticks[i]);
ModCom.RunModbus(ch2, (ushort)(times[i + 1] - times[i] * 1000));
}
// Set mode to sequence control
ModCom.RunModbus(Register.Mode, 51); // Set the mode to positonramp
bool done = false;
// Start to continously update the sequence registers
while (!done)
{
}
}
// Function to allow the user to manually control the motor, i.e the syring, from the console.
public void ManualControl()
{
// Write to the console
Console.WriteLine("Manual control active");
Console.WriteLine("Press enter to exit, + to increase and - to decrease position");
// Read current position from motor
int CurrentPosition = ModCom.ReadModbus(Register.Position, 2, true);
// Update last position to current position
int LastPosition = CurrentPosition;
// Define variable to save keyboard input from the user
ConsoleKeyInfo input = new ConsoleKeyInfo();
// As long as "Enter" is not pressed, continue
while (Console.ReadKey().Key != ConsoleKey.Enter)
{
// Read the keyboard input
input = Console.ReadKey(true);
// In input is "+", increase currenposition with 50
if (input.KeyChar == '+')
{
CurrentPosition = CurrentPosition + 50;
Console.WriteLine("position increased");
}
// In input is "-", decrease currenposition with 50
if (input.KeyChar == '-')
{
CurrentPosition = CurrentPosition - 50;
Console.WriteLine("position decreased");
}
// If currenposition was updated, update position of motor
if (CurrentPosition != LastPosition)
{
ModCom.RunModbus(Register.TargetInput, CurrentPosition);
LastPosition = CurrentPosition;
}
}
Console.WriteLine("Exiting manual control");
}
// Function for reading and saving the logged values
// Position = 1 Logging for position control
// Position = 0 Logging for velocity control
public void saveLoggedValues(int[] LogRecordedVelocities, int[] LogRecordedTargets,
int[] LogRecordedPressures, int[] LogRecordedLinearPositions,
int Position)
{
// Tmpty the lists for logged values, to avoid problem with lists growing
LoggedPressures = new List <Double>();
LoggedSensorValues = new List <int>();
// Save values from the log registers
for (ushort j = 0; j < 500; j++)
{
// Variables for increasing the register numbers
ushort ch1 = (ushort)(Register.LogRegisterValue1 + j);
ushort ch2 = (ushort)(Register.LogRegisterValue2 + j);
ushort ch3 = (ushort)(Register.LogRegisterValue3 + j);
ushort ch4 = (ushort)(Register.LogRegisterValue4 + j);
// Read values
LogRecordedVelocities[j] = ModCom.ReadModbus(ch1, 1, false);
LogRecordedTargets[j] = ModCom.ReadModbus(ch2, 1, false);
LogRecordedPressures[j] = ModCom.ReadModbus(ch3, 1, false);
LogRecordedLinearPositions[j] = ModCom.ReadModbus(ch4, 1, false);
LoggedSensorValues.Add(LogRecordedLinearPositions[j]);
LoggedPressures.Add((Double)LogRecordedPressures[j] - LogRecordedPressures[0]);
}
//Loop for converting LSB part of int32 to int16
for (int o = 0; o < 500; o++)
{
if (Position == 1)
{
byte[] bytes = BitConverter.GetBytes((ushort)LogRecordedVelocities[o]);
short y = BitConverter.ToInt16(bytes, 0);
LogRecordedVelocities[o] = (int)y;
}
byte[] bytes2 = BitConverter.GetBytes((ushort)LogRecordedTargets[o]);
short x = BitConverter.ToInt16(bytes2, 0);
LogRecordedTargets[o] = (int)x;
Console.Write("Target");
Console.WriteLine(LogRecordedTargets[o]);
Console.Write("Velocity");
Console.WriteLine(LogRecordedVelocities[o]);
}
}
// Function for initial test of linear sensor.
public void testLinearSensor()
{
// Write to the console
Console.WriteLine("Test of Linear sensor active!");
Console.WriteLine("Press enter to exit");
// Read current position from sensor
//int Position = ModCom.ReadModbus(Register.LinearPosition, 1, false);
//Console.WriteLine(Position);
// As long as "Enter" is not pressed, continue
//while (Console.ReadKey().Key != ConsoleKey.Enter)
while (true)
{
int Position = ModCom.ReadModbus(Register.LinearPosition, 1, false);
double PositionMM = sensorToLinPos(Position);
Console.WriteLine("PositionMM: ");
Console.WriteLine(PositionMM.ToString());
Console.WriteLine("Position: ");
Console.WriteLine(Position.ToString());
}
Console.WriteLine("Exiting manual control");
}
// Function for setting upp the logging of values from motor
// Position = 1 Logging for position control
// Position = 0 Logging for velocity control
public void setupLogging(int RegLogFactor, int Position)
{
// If position control
if (Position == 1)
{
// Define registers to log
ModCom.RunModbus(Register.LogRegister1, (ushort)(Register.Position + 1)); // Position
ModCom.RunModbus(Register.LogRegister2, (ushort)(Register.TargetInput + 1)); // Regulator target
ModCom.RunModbus(Register.LogRegister3, Register.Pressure); // Pressure
ModCom.RunModbus(Register.LogRegister4, Register.LinearPosition); // Linear position
}
else
{
// Define registers to log
ModCom.RunModbus(Register.LogRegister1, Register.Speed); // Speed
ModCom.RunModbus(Register.LogRegister2, (ushort)(Register.TargetInput + 1)); // Regulator target
ModCom.RunModbus(Register.LogRegister3, Register.Pressure); // Pressure
ModCom.RunModbus(Register.LogRegister4, Register.LinearPosition); // Linear position
}
// Settings for the logging
ModCom.RunModbus(Register.LogPeriod, (short)RegLogFactor);
// Start logging
ModCom.RunModbus(Register.LogState, (short)2);
}
public void volumeTest()
{
// Define variable to save keyboard input from the user
ConsoleKeyInfo input = new ConsoleKeyInfo();
Console.WriteLine("Press Enter to start the volume test");
// Start the volume test when enter is pressed
while (Console.ReadKey().Key != ConsoleKey.Enter)
{
}
// Initialize the motor
ModCom.RunModbus(Register.Mode, Mode.MotorOff); // Turn off the motor
ModCom.RunModbus(Register.Speed, (Int32)0); // Set the speed to 0
ModCom.RunModbus(Register.Position, (Int32)0); // Set the position to 0
ModCom.RunModbus(Register.TargetInput, (Int32)0); // Set the target to 0
ModCom.RunModbus(Register.Mode, Mode.PositionRamp); // Set the mode to positonramp
// Get the current linear position
double currentPosition = sensorToLinPos(ModCom.ReadModbus(Register.LinearPosition, 1, false));
double homePos = currentPosition;
double dist = 3.304 + homePos;
int newPosition = 0;
bool done = false;
Console.Write("Start Position in MM: ");
Console.WriteLine(currentPosition);
Console.Write("Start Position: ");
Console.WriteLine(ModCom.ReadModbus(Register.LinearPosition, 1, false));
while (!done)
{
// Read the current position
currentPosition = sensorToLinPos(ModCom.ReadModbus(Register.LinearPosition, 1, false));
// Define variable for how much to inrease
int increase = 0;
// Decide how much to increase with depending on how far away from home
if (Math.Abs(currentPosition - dist) > 0.5)
{
increase = 20;
}
else
{
increase = 1;
}
// If the piston is to far into the syringe, turn the motor counterclockwise (piston is moved 0.mm)
if (currentPosition < dist - Hardware.HomePosTolerance)
{
// Increase the position
newPosition = newPosition - increase;
}
// Check if we are done
else if (currentPosition > dist - Hardware.HomePosTolerance)
{
// Decrease the position
done = true;
}
// Write the new position to the motor
ModCom.RunModbus(Register.TargetInput, newPosition);
}
// Turn off the motor
ModCom.RunModbus(Register.Mode, Mode.MotorOff);
// Set the position to 0
ModCom.RunModbus(Register.Position, (Int32)0);
// Set the target to 0
ModCom.RunModbus(Register.TargetInput, (Int32)0);
Console.Write("End Position in MM: ");
Console.WriteLine(currentPosition);
Console.Write("End Position: ");
Console.WriteLine(ModCom.ReadModbus(Register.LinearPosition, 1, false));
}
// Function to initialize the motor and set the system in its home position
public void goToHome()
{
// Tune the regulator
ModCom.RunModbus(300, (Int16)1000); //P
ModCom.RunModbus(301, (Int16)1000); //I
ModCom.RunModbus(302, (Int16)0); //D
// Get the current linear position
double currentPosition = sensorToLinPos(ModCom.ReadModbus(Register.LinearPosition, 1, false));
// Chech if the current linear position is valid, if not throw an error
if (currentPosition > Hardware.MaxPosition | currentPosition < Hardware.MinPosition)
{
throw new Exception("Measurement from linear sensor is out of physical range!");
}
// Boolean operator to keep track if home position found
bool done = false;
// Define value for the updated position for the motor
int newPosition = 0;
// Initialize the motor
ModCom.RunModbus(Register.Mode, Mode.MotorOff); // Turn off the motor
ModCom.RunModbus(Register.Speed, (Int32)0); // Set the speed to 0
ModCom.RunModbus(Register.Position, (Int32)0); // Set the position to 0
ModCom.RunModbus(Register.TargetInput, (Int32)0); // Set the target to 0
ModCom.RunModbus(Register.Mode, Mode.PositionRamp); // Set the mode to positonramp
// Run the motor until the system is in its homeposition
while (!done)
{
// Read the current position
currentPosition = (Double)sensorToLinPos(ModCom.ReadModbus(Register.LinearPosition, 1, false));
// Define variable for how much to inrease
int increase = 0;
// Decide how much to increase with depending on how far away from home
if (Math.Abs(currentPosition - Hardware.HomePosition) > 0.5)
{
increase = 20;
}
else
{
increase = 1;
}
// If the piston is to far into the syringe, turn the motor counterclockwise (piston is moved 0.mm)
if (currentPosition > Hardware.HomePosition + Hardware.HomePosTolerance)
{
// Increase the position
newPosition = newPosition + increase;
}
// If the piston is to far out of the syringe, turn the motor clockwise
else if (currentPosition < Hardware.HomePosition - Hardware.HomePosTolerance)
{
// Decrease the position
newPosition = newPosition - increase;
}
// Check if we are done
if (currentPosition <Hardware.HomePosition + Hardware.HomePosTolerance&
currentPosition> Hardware.HomePosition - Hardware.HomePosTolerance)
{
done = true;
}
Console.Write("Current Position: ");
Console.WriteLine(currentPosition);
// Write the new position to the motor
ModCom.RunModbus(Register.TargetInput, newPosition);
}
Console.Write("Current Position: ");
Console.WriteLine(currentPosition);
// Turn off the motor
ModCom.RunModbus(Register.Mode, Mode.MotorOff); // Set the mode to positonramp
// Set the position to 0
ModCom.RunModbus(Register.Position, (Int32)0);
// Set the target to 0
ModCom.RunModbus(Register.TargetInput, (Int32)0);
}
// Function to actually run the motor based on a list of ticks, i.e a list with angular positions
// for the motor, a list of times and a given mode.
public void RunTickSequence(List <Int32> ticks, List <Double> times, Int16 mode)
{
// If the number of ticks and number of times are not equal, throw an error
if (ticks.Count() != times.Count())
{
throw new Exception("Input lists not of equal length");
}
// Create event reading the maximum torque status register
CreateEvent((ushort)0,
(Int16)(0B000000000100000), // Bitmask to get torque from status register
(Int16)(MotorControl.Register.Status),
(ushort)0XF007, // Logical 'and' between bitmask and status register
(Int16)(MotorControl.Register.Mode),
(ushort)0,
(Int16)0); // No source register
// Set a maximum allowed torque
ModCom.RunModbus(MotorControl.Register.MotorTorqueMax, Hardware.MaxTorque);
// Define the sequence length
int sequenceLength = ticks.Count();
// Create an instance of Stopwatch used to measure the time
Stopwatch stopWatch = new Stopwatch();
// Define lists to save logged motor values
Int32[] LogRecordedVelocities = new Int32[500];
Int32[] LogRecordedTargets = new Int32[500];
Int32[] LogRecordedPressures = new Int32[500];
Int32[] LogRecordedLinearPositions = new Int32[500];
// Define list for the actual time
Double[] StopwatchRecordedTimes = new Double[sequenceLength];
// Add time values to LoggedTime and compute the LogFactor for logging
int RegLogFactor = createTimeVector(times, sequenceLength);
// Run the system to its home position
goToHome();
// SET the PID-parameters
ModCom.RunModbus(300, (Int16)10000); //P
ModCom.RunModbus(301, (Int16)1000); //I
ModCom.RunModbus(302, (Int16)0); //D
// Initializing the motor
ModCom.RunModbus(Register.Mode, Mode.MotorOff); // Turn off the motor
ModCom.RunModbus(Register.Speed, (Int32)0); // Set the speed to 0
ModCom.RunModbus(Register.Position, (Int32)0); // Set the position to 0
ModCom.RunModbus(Register.TargetInput, (Int32)0); // Set the motor to be continously controlled via communication bus
// Set the mode of the motor to the given mode
ModCom.RunModbus(Register.Mode, mode);
// Set time = 0 in the motor time register
ModCom.RunModbus(Register.Time, (Int32)0);
// Setup and start the logging
setupLogging(RegLogFactor, 0);
// Start to run the actual sequence and send regulator targets to the motor
int i = 0;
// Start to measure time at computer
stopWatch.Start();
while (i < sequenceLength)
{
// If more time have elapsed than times[i], send a new regulator target to the motor
if (times[i] <= stopWatch.Elapsed.TotalSeconds)
{
// Write a new target value to the motor
ModCom.RunModbus(Register.TargetInput, (Int32)ticks[i]);
// Read time
StopwatchRecordedTimes[i] = stopWatch.Elapsed.TotalSeconds;
i++;
}
}
// Set target to zero
ModCom.RunModbus(Register.TargetInput, (Int32)0);
// Turn off the motor
ModCom.RunModbus(Register.Mode, Mode.MotorOff);
// Stop counting the time
stopWatch.Stop();
// Read and save logged values from motor
saveLoggedValues(LogRecordedVelocities, LogRecordedTargets,
LogRecordedPressures, LogRecordedLinearPositions, 1);
// Convert Ticks to positions
LoggedVelocities = MotorSpeedToVelocity(LogRecordedVelocities);
// Convert Ticks to velocity
LoggedTargets = TicksPerSecondToVelocity(LogRecordedTargets);
// Convert sensorreading to linear position
LoggedLinearPositions = sensorToLinPosList(LogRecordedLinearPositions);
//LoggedPressures = uns16ToVDc(LogRecordedPressures);
}
