#include <Math.h> // For math functions
#include <PololuMaestro.h>
MiniMaestro maestro(Serial1);
// Define the analog input pin for the trigger signal
const int triggerPin = A0; // Use an analog pin for voltage signals
volatile bool triggerDetected = false; // Flag to indicate trigger detection
int currentPrecalculatedIndex = 0; // Keep track of the current precalculated position
//a few definitions
struct MotorPosition {
double Motor1;
double Motor2;
double Motor3;
};
MotorPosition motorPositions;
MotorPosition PrecalculatedPositions[200]; // Declare PrecalculatedPositions as a global array
double AlphaBeta[][2] = {
{-10.72, -9},
{-7.86, -9.07},
{-4.76, -8.79},
{-1.9, -8.8},
{0.97, -8.95},
{3.97, -9.18},
}
void InitializePrecalculatedPositions() {
int index = 0;
int numEntries = sizeof(AlphaBeta) / (sizeof(AlphaBeta[0]) * 2);
for (int i = 0; i < numEntries; i++) {
double alpha = AlphaBeta[i][0];
double beta = AlphaBeta[i][1];
// Calculate and store motor positions
calculateMotorPositionFromDegree(alpha, beta, maximumValue, minimumValue);
PrecalculatedPositions[index] = motorPositions;
index++;
}
}
void setup() {
// Initialize Serial communication with the Maestro controller
Serial1.begin(9600);
// Set up the external trigger pin for interrupt on rising edge
pinMode(triggerPin, INPUT);
attachInterrupt(digitalPinToInterrupt(triggerPin), handleTrigger, RISING);
InitializePrecalculatedPositions();
}
void loop()
{
if (triggerDetected) {
if (currentPrecalculatedIndex < sizeof(PrecalculatedPositions) / sizeof(PrecalculatedPositions[0])) {
// Send the calculated positions to the Maestro controller one by one
MotorPosition position = PrecalculatedPositions[currentPrecalculatedIndex];
// Convert angle to microseconds using your conversion function
int targetMicros1 = angleToMicroseconds(position.Motor1, nullPositionMotor1);
int targetMicros2 = angleToMicroseconds(position.Motor2, nullPositionMotor2);
int targetMicros3 = angleToMicroseconds(position.Motor3, nullPositionMotor3);
// Set the target positions using the maestro.setTarget function
maestro.setTarget(channelMotor1, targetMicros1);
maestro.setTarget(channelMotor2, targetMicros2);
maestro.setTarget(channelMotor3, targetMicros3);
currentPrecalculatedIndex++;
}
// Reset the trigger detection flag
triggerDetected = false;
}
// Your other code can continue here
}
int angleToMicroseconds(double angle, double nullPosition)
{
// Convert the angle to microseconds using your custom conversion logic
double oneDegreeInMicroSec = 8.33; // Modify this value as needed
int targetMicros = nullPosition + (angle * oneDegreeInMicroSec);
return targetMicros;
}
Above is the code that I wrote in arduino, to control the servo motors. Each time there’s a trigger recieved, it has to keep moving through the precalculated positions.
I have connected the tx of arduino to rx of maestro. How should my c# code be for maestro to run?
public void InitializeSerialPort()
{
mySerialPort = new SerialPort("COM10");
mySerialPort.BaudRate = 9600;
mySerialPort.Parity = Parity.None;
mySerialPort.StopBits = StopBits.One;
mySerialPort.DataBits = 8;
mySerialPort.Handshake = Handshake.None;
mySerialPort.RtsEnable = true;
try
{
mySerialPort.Open();
MessageBox.Show("Serial port opened successfully.");
}
catch (UnauthorizedAccessException ex)
{
MessageBox.Show("Unauthorized access to the serial port. Check if the port is already in use by another application.");
}
catch (IOException ex)
{
MessageBox.Show("An I/O error occurred while opening the serial port.");
}
catch (ArgumentException ex)
{
MessageBox.Show("Invalid serial port settings or name.");
}
}
public void CloseSerialPort()
{
if (mySerialPort != null && mySerialPort.IsOpen)
{
mySerialPort.Close();
MessageBox.Show("Serial port closed successfully.");
}
}
public virtual void OnTriggerReceived()
{
if (servoController.IsConnected)
{
// Check if the event has subscribers (is not null)
if (TriggerReceived != null)
{
TriggerReceived?.Invoke(this, EventArgs.Empty);
//MessageBox.Show("Trigger received ontriggerrecieve()!");
}
}
}
public void AttachDataReceivedHandler()
{
if (servoMotorHandler != null && mySerialPort != null && mySerialPort.IsOpen)
//if (servoController.IsConnected)
{
mySerialPort.DataReceived += new SerialDataReceivedEventHandler(Maestro_TriggerReceived);
}
else
{
// Log an error or add debugging information
mainWindow.Log("servoMotorHandler or mySerialPort is null.");
}
}
private void Maestro_TriggerReceived(object sender, SerialDataReceivedEventArgs e)
{
mainWindow.allowTrigger = true;
SerialPort sp = (SerialPort)sender;
while (sp.BytesToRead >= 12)
{
byte[] buffer = new byte[12];
sp.Read(buffer, 0, 12);
double motorPosition1 = BitConverter.ToDouble(buffer, 0);
double motorPosition2 = BitConverter.ToDouble(buffer, 8);
double motorPosition3 = BitConverter.ToDouble(buffer, 16);
// Set the motor positions based on the received values
servoMotorHandler.SetMotorPositionsAndSpeed(motorPosition1, motorPosition2, motorPosition3, servoMotorHandler.speed);
// Update the UI with the received positions
mainWindow.BeginInvoke(new Action(() =>
{
mainWindow.PositionTextBox1.Text = motorPosition1.ToString();
mainWindow.PositionTextBox2.Text = motorPosition2.ToString();
mainWindow.PositionTextBox3.Text = motorPosition3.ToString();
}));
}
OnTriggerReceived();
}
Does it work like this?
