Arduino-maestro control

#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?

5 posts - 2 participants

Read full topic