// Function to set a maximum torque to the motor and to create lists for
// values to be read from motor controller.
public MotorControl(ModbusCommunication modCom)
{
ModCom = modCom;
// Create empty lists to store values that should be logged from the motor controller
LoggedVelocities = new List <Double>();
LoggedTargets = new List <Double>();
LoggedPressures = new List <Double>();
LoggedLinearPositions = new List <Double>();
LoggedSensorValues = new List <int>();
// Create empty list to store the time vector corresponding to logged values
LoggedTime = new List <Double>();
// 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);
}
// The main program that runs everything
public static void Main(string[] args)
{
// Get the COM-port that the motor is connected to
String portName = ModbusCommunication.GetSerialPortName();
// Create an instance of the ModbusCommunication
ModbusCommunication modCom = new ModbusCommunication(portName);
// Reset all running data and then enter Off-mode for the motor
modCom.RunModbus(Register.Mode, (Int16)1);
// Set the target to 0
modCom.RunModbus(Register.TargetInput, 0);
// Set the mode to position ramp (closed control of position with ramp)
//modCom.RunModbus(Register.Mode, (Int16)21);
//modCom.RunModbus(Register.TargetInput,4096);
//modCom.RunModbus(Register.Mode,(Int16)33);
//modCom.RunModbus(Register.TargetInput,100);
//Thread.Sleep(2000);
//modCom.RunModbus(Register.TargetInput,2000);
//Thread.Sleep(2000);
//modCom.RunModbus(Register.TargetInput,8000);
//Thread.Sleep(2000);
//modCom.RunModbus(Register.TargetInput,30000);
//Thread.Sleep(2000);
//modCom.RunModbus(Register.TargetInput,2000);
//Thread.Sleep(2000);
//modCom.RunModbus(Register.TargetInput,0);
//modCom.RunModbus(Register.Mode,(Int16)1);
// Create an instance o´f MotorControl
MotorControl motCon = new MotorControl(modCom);
// Test of event safety function
int currentTorque = modCom.ReadModbus(Register.Torque, (ushort)1, false);
Console.WriteLine("current Torque:");
Console.WriteLine(currentTorque);
// Create event that gets the torque
motCon.CreateEvent((ushort)0,
(Int16)(0B000000000100000), // Bitmask to get torque from status register
(Int16)(Register.Status),
(ushort)0XF007, // AND between bitmask and status register
(Int16)(Register.Mode),
(ushort)0,
(Int16)0); // No source register
// Set the max torque to 100 nNm
modCom.RunModbus(Register.MotorTorqueMax, (Int16)300);
// Variable for dummie read
int dummieRead;
// Lists for recorded values
Double[] RecordedTimes1 = new Double[100];
Double[] RecordedTimes2 = new Double[100];
Double[] RecordedTimesRead = new Double[100];
Double[] RecordedTimesWrite = new Double[100];
// Start to measure time
Stopwatch stopWatch = new Stopwatch();
stopWatch.Start();
// The two loops below checks how long time it takes to read from the motor controller
for (int i = 0; i < 99; i++)
{
RecordedTimes1[i] = stopWatch.Elapsed.TotalSeconds;
dummieRead = modCom.ReadModbus(Register.Position, 2, true);
RecordedTimes2[i] = stopWatch.Elapsed.TotalSeconds;
}
for (int i = 0; i < 100; i++)
{
RecordedTimesRead[i] = RecordedTimes2[i] - RecordedTimes1[i];
}
// The two loops below checks how long time it takes to write from the motor controller
for (int i = 0; i < 100; i++)
{
RecordedTimes1[i] = stopWatch.Elapsed.TotalSeconds;
modCom.RunModbus((ushort)450, (int)0);
RecordedTimes2[i] = stopWatch.Elapsed.TotalSeconds;
}
for (int i = 0; i < 100; i++)
{
RecordedTimesWrite[i] = RecordedTimes2[i] - RecordedTimes1[i];
}
// Write the recorded read and write times to the console
//Console.WriteLine("Read: Max: {0}, Min: {1}, Avr: {2}", RecordedTimesRead.Max(), RecordedTimesRead.Min(), RecordedTimesRead.Average());
//Console.WriteLine("Write: Max: {0}, Min: {1}, Avr: {2}", RecordedTimesWrite.Max(), RecordedTimesWrite.Min(), RecordedTimesWrite.Average());
// Define a test sequence
//List<Int32> ticks = new List<Int32>() { 0, 100, 1000, 2000, 3000, 2000, 1000, 100, 0 };
//List<Int32> ticks = new List<Int32>() {0,2000,4000,8000,4000,500,-2000,-2000,0};
//List<double> times = new List<double>() { 0, 1, 2, 3, 4, 5, 6, 7, 8 };
// Run the test sequence
//motCon.RunTickSequence(ticks, times, Mode.PositionRamp);
//motCon.ManualControl();
//Console.ReadLine();
double currentTime = stopWatch.Elapsed.TotalSeconds;
double lastTime = stopWatch.Elapsed.TotalSeconds;
//List<Double> times = new List<Double>() {0, 1.0, 2.0, 3.0, 4.0, 5.0}; ;
//int test = motCon.createTimeVector(times, times.Count());
//Console.Write("RegLogFactor: ");
//Console.Write(test);
//motCon.testLinearSensor();
//motCon.testLinearSensor();
motCon.goToHome();
motCon.volumeTest();
//Console.WriteLine("Changing baudrate");
//modCom.RunModbus(52, (Int16)260);
// Tune the regulator
//modCom.RunModbus(300, (Int16)100); //P
//modCom.RunModbus(301, 0); //I
//modCom.RunModbus(302, 0); //D
//Console.Write("P-parameter:");
//Console.WriteLine(modCom.ReadModbus(300, 1, false));
//Console.Write("I-parameter:");
//Console.WriteLine(modCom.ReadModbus(301, 1, false));
//Console.Write("D-parameter:");
//Console.WriteLine(modCom.ReadModbus(302, 1, false));
//Console.Write("Friction-parameter:");
//Console.WriteLine(modCom.ReadModbus(305, 1, false));
modCom.EndModbus();
Console.ReadLine();
}
