// 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");
}
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);
}