6DOF – Stewart platform mit FSX –

6DOF – Stewart platform mit FSX –

Hier mal ein erster Test einer 6DOF Stewart Plattform auf Servo und arduino-basis in Verbindung mit SIMTOOLS.

/*
//********************************************************************************************
// RC Model Servo
// Original code By EAOROBBIE (Robert Lindsay)
// Completely mangled by aarondc
// For free use for Sim Tool Motion Software
// Changed by SIM-PC.de for 6DOF
//********************************************************************************************
#include 
//#define DEBUG 1               // comment out this line to remove debuggin Serial.print lines
const int kActuatorCount = 6;   // how many Actuators we are handling

// the letters ("names") sent from Sim Tools to identify each actuator
// NB: the order of the letters here determines the order of the remaining constants kPins and kActuatorScale
const char kActuatorName[kActuatorCount] = { 'A', 'B', 'C', 'D', 'E', 'F' };
const int kPins[kActuatorCount] = {2, 3, 4, 5, 6, 7};           
const int kActuatorScale[kActuatorCount][6] = { { 0, 179 } ,    // 1 Actuator scaling
                                                { 0, 179 } ,    // 2 Actuator scaling
                                                { 0, 179 } ,    // 3 Actuator scaling
                                                { 0, 179 } ,    // 4 Actuator scaling
                                                { 0, 179 } ,    // 5 Actuator scaling
                                                { 0, 179 }      // 6 Actuator scaling
                                               };  
const char kEOL = '~';                              // End of Line - the delimiter for our acutator values 
const int kMaxCharCount = 3;                        // some insurance...
Servo actuatorSet[kActuatorCount];                  // our array of Actuators
int actuatorPosition[kActuatorCount] = {90,90,90,98,90,90};    // current Actuator positions, initialised to 90
int currentActuator;                                // keep track of the current Actuator being read in from serial port
int valueCharCount = 0;                             // how many value characters have we read (must be less than kMaxCharCount!!

// set up some states for our state machine
// psReadActuator = next character from serial port tells us the Actuator
// psReadValue = next 3 characters from serial port tells us the value
enum TPortState { psReadActuator, psReadValue };   
TPortState currentState = psReadActuator;

void setup()
{ 
    // attach the Actuators to the pins
    for (int i = 0; i < kActuatorCount; i++) 
        actuatorSet[i].attach(kPins[i]);
    
    // initialise actuator position
    for (int i = 0; i < kActuatorCount; i++) updateActuator(i); Serial.begin(38400); // opens serial port at a baud rate of 9600 } void loop() { } // this code only runs when we have serial data available. ie (Serial.available() > 0).
void serialEvent() {
    char tmpChar;
    int tmpValue;

    while (Serial.available()) {
        // if we're waiting for a Actuator name, grab it here
        if (currentState == psReadActuator) {
            tmpChar = Serial.read();
            // look for our actuator in the array of actuator names we set up 
#ifdef DEBUG            
Serial.print("read in ");            
Serial.println(tmpChar);            
#endif
            for (int i = 0; i < kActuatorCount; i++) {
                if (tmpChar == kActuatorName[i]) {
#ifdef DEBUG            
Serial.print("which is actuator ");            
Serial.println(i);            
#endif
                    currentActuator = i;                        // remember which actuator we found
                    currentState = psReadValue;                 // start looking for the Actuator position 
                    actuatorPosition[currentActuator] = 0;      // initialise the new position
                    valueCharCount = 0;                         // initialise number of value chars read in 
                    break;
                }
            }
        }
        
        // if we're ready to read in the current Actuator's position data
        if (currentState == psReadValue) {
            while ((valueCharCount < kMaxCharCount) && Serial.available()) {
                tmpValue = Serial.read();
                if (tmpValue != kEOL) {
                    tmpValue = tmpValue - 48;
                    if ((tmpValue < 0) || (tmpValue > 9)) tmpValue = 0;
                    actuatorPosition[currentActuator] = actuatorPosition[currentActuator] * 10 + tmpValue;
                    valueCharCount++;
                }
                else break;
            }
            
            // if we've read the value delimiter, update the Actuator and start looking for the next Actuator name
            if (tmpValue == kEOL || valueCharCount == kMaxCharCount) {
#ifdef DEBUG            
Serial.print("read in ");            
Serial.println(actuatorPosition[currentActuator]);            
#endif
                // scale the new position so the value is between 0 and 179
                actuatorPosition[currentActuator] = map(actuatorPosition[currentActuator], 0, 255, kActuatorScale[currentActuator][0], kActuatorScale[currentActuator][1]);
#ifdef DEBUG            
Serial.print("scaled to ");            
Serial.println(actuatorPosition[currentActuator]);            
#endif
                updateActuator(currentActuator);
                currentState = psReadActuator;
            }
        }
    }
}


// write the current Actuator position to the passed in Actuator 
void updateActuator(int thisActuator) {
    actuatorSet[thisActuator].write(actuatorPosition[thisActuator]);
}