D:/Projekte/Super-Yano/Super-Yano/RobotControl.cpp

gehe zur Dokumentation dieser Datei

#include "StdAfx.h"
#include "RobotControl.h"
#include <ctype.h>

void RobotControl::construct(System::String^ port_name) 
{
        blinkingState = STOPPED;
        earTremorState = STOPPED;
        visualAdjustmentThreadState = STOPPED;
        isMouthMovement = true;
        
        initiateServoSettings();
        
        components = (gcnew System::ComponentModel::Container());

        spVisemeEventHandler = gcnew SpeechLib::_ISpeechVoiceEvents_VisemeEventHandler(&RobotControl::speechVisemeEvent);

        isSerialPort = true;
                
        serial_Port = (gcnew System::IO::Ports::SerialPort(components));
        
        try
        {
                serial_Port->BaudRate = 2400;
                serial_Port->PortName = port_name;
                serial_Port->DataReceived += gcnew System::IO::Ports::SerialDataReceivedEventHandler(&RobotControl::serialPort_DataReceived);
                serial_Port->Encoding = System::Text::Encoding::GetEncoding(28591);
                serial_Port->NewLine = "\r";
        }
        catch (Exception^ e)
        {
                serial_Port->Close();
                isSerialPort = false;
                Windows::Forms::MessageBox::Show( e->Message + "\nEs trat ein Fehler beim Konfigurieren des COM-Objets auf!", "Comport-Fehler "+port_name, Windows::Forms::MessageBoxButtons::OK, Windows::Forms::MessageBoxIcon::Error);
        }


        //nach Roboter suchen...
        String^ s = findComport(port_name);
        if (s->CompareTo("") != 0)
        {
                serial_Port->PortName = s;
                isSerialPort = true;
        }
        else
        {
                Windows::Forms::MessageBox::Show( "Es wurde kein Roboterinterface gefunden!\n", "Comport-Fehler", Windows::Forms::MessageBoxButtons::OK, Windows::Forms::MessageBoxIcon::Error);
        }

        
        //Baudrate auf 38400 Baud setzen
        if(isSerialPort == true)
        {
                try
                {
                        if (!serial_Port->IsOpen) serial_Port->Open();
                        serial_Port->WriteLine("2400 Baud");
                        serial_Port->WriteLine("U!SCVER?");
                        Threading::Thread::Sleep(1500);
                        serial_Port->WriteLine("U!SCSBR"+Char(0x01));
                        serial_Port->Close();
                        serial_Port->BaudRate = 38400;
                        serial_Port->DataReceived += gcnew System::IO::Ports::SerialDataReceivedEventHandler(&RobotControl::serialPort_DataReceived);
                        serial_Port->Open();
                        serial_Port->Encoding = System::Text::Encoding::GetEncoding(28591);
                        serial_Port->NewLine = "\r";    
                        serial_Port->WriteLine("U!SCVER?");
                        Threading::Thread::Sleep(500);
                        initiateRobot();
                        Threading::Thread::Sleep(500);
                }
                catch (Exception^ e)
                {
                        serial_Port->Close();
                        isSerialPort = false;
                        Windows::Forms::MessageBox::Show( e->Message + "\nDie Com-Schnittstelle "+ getComPortName() +" konnte nicht initialisiert werden.\nDer Roboterkopf wird nicht benutzt.", "Comport-Fehler", Windows::Forms::MessageBoxButtons::OK, Windows::Forms::MessageBoxIcon::Error);
                }
        }


        blinkThread = gcnew Thread( gcnew ThreadStart( &RobotControl::blink ));
        blinkingState = RUNNING;
        blinkThread->Start();

        earTremorThread = gcnew Thread( gcnew ThreadStart( &RobotControl::earTremorISR ));
        earTremorState = RUNNING;
        earTremorThread->Start();

        visualAdjustmentThread = gcnew Thread( gcnew ThreadStart( &RobotControl::visualAdjustmentThreadISR ));
        visualAdjustmentThreadState = RUNNING;
        visualAdjustmentThread->Start();


}

void RobotControl::setBlinkingThreadPaused(bool paused)
{
        if((paused)&&(blinkingState == RUNNING)) blinkingState = PAUSED;
        else if((!paused)&&(blinkingState == PAUSED)) blinkingState = RUNNING;
}

void RobotControl::setEarTremorThreadPaused(bool paused)
{
        if((paused)&&(earTremorState == RUNNING)) earTremorState = PAUSED;
        else if((!paused)&&(earTremorState == PAUSED)) earTremorState = RUNNING;
}

void RobotControl::setVisualAdjustmentThreadPaused(bool paused)
{
        if((paused)&&(visualAdjustmentThreadState == RUNNING)) visualAdjustmentThreadState = PAUSED;
        else if((!paused)&&(visualAdjustmentThreadState == PAUSED)) visualAdjustmentThreadState = RUNNING;
}

void RobotControl::setMouthMovementEnabled(bool enabled)
{
        isMouthMovement = enabled;
}


void RobotControl::setCamera(int camera)
{
        if ( serial_Port->IsOpen)
        {
                used_eye = camera;
                serial_Port->WriteLine("!CAM"+camera);
        }
}

void RobotControl::setEyeOpen(int eye, bool eyeOpened)
{
        if (eye == LEFT)
        {
                if (eyeOpened)
                {
                        setServoValue(lid_l, 0, servos[lid_l]->startValue);
                }
                else
                {
                        setServoValue(lid_l, 0, servos[lid_l]->maxValue);
                }
        }
        else if (eye == RIGHT)
        {
                if (eyeOpened)
                {
                        setServoValue(lid_r, 0, servos[lid_r]->startValue);     
                }
                else
                {
                        setServoValue(lid_r, 0, servos[lid_r]->minValue);
                }               
        }
}

void RobotControl::visualAdjustmentThreadISR(void)
{
        int numberOfFramesWithoutFaces = 0;
        int maxNumberOfFramesWithoutFaces = 10;

        int numberOfFramesSinceLastEyeMove = 0;
        int maxNumberOfFramesSinceLastEyeMove = 5;

        int eyeSleepTime = 25;

        double currentRelativeX;
        double currentRelativeY;

        //double diffX, diffY;

        while(visualAdjustmentThreadState != STOPPED)
        {

                while(visualAdjustmentThreadState == RUNNING)
                {
                        Thread::Sleep(10);

                        int count = getNumberOfFaces();
                        double fx = getMajorFaceRelativeX(); 
                        double fy = getMajorFaceRelativeY(); 

                        if(count < 1)  // kein Gesicht gefunden
                        {
                                if (numberOfFramesWithoutFaces >= maxNumberOfFramesWithoutFaces)
                                {
                                        //manchmal verhackeln sich die Augenlider, darum vielleicht öffnen
                                        setEyeOpen(LEFT, true);
                                        Threading::Thread::Sleep(50);
                                        setEyeOpen(RIGHT, true);
                                }

                                if (numberOfFramesWithoutFaces < maxNumberOfFramesWithoutFaces)
                                {
                                        numberOfFramesWithoutFaces++;
                                }
                        }
                        else  //mindestens ein Gesicht gefunden
                        {
                                numberOfFramesWithoutFaces = 0;

                                numberOfFramesSinceLastEyeMove++;

                                currentRelativeX = (servos[turn]->currentValue - servos[turn]->minValue) / (servos[turn]->maxValue - servos[turn]->minValue);
                                currentRelativeY = (servos[pitch]->currentValue - servos[pitch]->minValue) / (servos[pitch]->maxValue - servos[pitch]->minValue);
                                
                                //diffX = Math::Abs(fx - currentRelativeX);
                                //diffY = Math::Abs(fy - currentRelativeY);

                                if ( Math::Abs(fx - 0.5) > 0.07) // minus 50%, um von 43% auf -7% bzw. von 57% auf 7% zu kommen, weil ja Abweichung vom Bildmittelpunkt (50%) gesucht ist
                                {
                                        if(fx > 0.5)
                                        {
                                                        if(servos[eye_l]->currentValue >= (servos[eye_l]->minValue+20))
                                                        {
                                                                setServoValue(eye_l, 0, servos[eye_l]->currentValue - 20); 
                                                                numberOfFramesSinceLastEyeMove = 0;
                                                                Thread::Sleep(eyeSleepTime);
                                                        }
                                                        if(servos[eye_r]->currentValue >= (servos[eye_r]->minValue+20))
                                                        {
                                                                setServoValue(eye_r, 0, servos[eye_r]->currentValue - 20); 
                                                                numberOfFramesSinceLastEyeMove = 0;
                                                                Thread::Sleep(eyeSleepTime);
                                                        }
                                        }
                                        else // diffX <= 0.5
                                        {
                                                        if(servos[eye_l]->currentValue <= (servos[eye_l]->maxValue-20))
                                                        {
                                                                setServoValue(eye_l, 0, servos[eye_l]->currentValue + 20); 
                                                                numberOfFramesSinceLastEyeMove = 0;
                                                                Thread::Sleep(eyeSleepTime);
                                                        }
                                                        if(servos[eye_r]->currentValue <= (servos[eye_r]->maxValue-20))
                                                        {
                                                                setServoValue(eye_r, 0, servos[eye_r]->currentValue + 20); 
                                                                numberOfFramesSinceLastEyeMove = 0;
                                                                Thread::Sleep(eyeSleepTime);
                                                        }
                                                        
                                        } // fx <= 0.5

                                } // fx ausserhalb toleranz?

                                if (numberOfFramesSinceLastEyeMove > maxNumberOfFramesSinceLastEyeMove)
                                {
                                        if (
                                                        (
                                                                (used_eye == LEFT) 
                                                                && 
                                                                ( Math::Abs(servos[eye_l]->currentValue - servos[eye_l]->startValue) > 100 )
                                                        )
                                                        ||
                                                        (
                                                                (used_eye == RIGHT) 
                                                                && 
                                                                ( Math::Abs(servos[eye_r]->currentValue - servos[eye_r]->startValue) > 100 )
                                                        )
                                                )
                                        {
                                                int direction;
                                                if(used_eye == RIGHT)
                                                {
                                                        if(servos[eye_r]->currentValue > servos[eye_r]->startValue) direction = RIGHT;
                                                        else direction = LEFT;
                                                }
                                                else
                                                {
                                                        if(servos[eye_l]->currentValue > servos[eye_l]->startValue) direction = RIGHT;
                                                        else direction = LEFT;
                                                }
                                                if(direction == RIGHT)
                                                {
                                                        if(servos[turn]->currentValue >= (servos[turn]->minValue + 5))
                                                        {
                                                                setServoValue(turn, 0, servos[turn]->currentValue - 5); 
                                                                Thread::Sleep(20);
                                                        }
                                                }
                                                else
                                                {
                                                        if(servos[turn]->currentValue <= (servos[turn]->maxValue - 5))
                                                        {
                                                                setServoValue(turn, 0, servos[turn]->currentValue + 5); 
                                                                Thread::Sleep(20);
                                                        }
                                                }
                                        } // if .x.x.x.x.x.x.x.
                                } // if (numberOfFramesSinceLastEyeMove > maxNumberOfFramesSinceLastEyeMove)


                                if ( Math::Abs(fy - 0.5) > 0.15)
                                {
                                        if(fy > 0.5)
                                        {
                                                if(servos[pitch]->currentValue >= (servos[pitch]->minValue+20))
                                                {
                                                        setServoValue(pitch, 0, servos[pitch]->currentValue - 20); 
                                                        Thread::Sleep(20);
                                                }
                                        }
                                        else
                                        {
                                                if(servos[pitch]->currentValue <= (servos[pitch]->maxValue-20))
                                                {
                                                        setServoValue(pitch, 0, servos[pitch]->currentValue + 20); 
                                                        Thread::Sleep(20);
                                                }
                                        }
                                } // fy ausserhalb toleanz?


                        }

                } //while RUNNING

                Thread::Sleep(100);

        } //while != STOPPED

}

void RobotControl::earTremorISR(void)
{

        long sleepTime;

        Random^ rnd = gcnew Random();

        while(earTremorState != STOPPED)
        {
                while(earTremorState == RUNNING)
                {
                        sleepTime = rnd->Next(30000, 90000);
                        for(int i=0; i< (sleepTime/100); i++)
                        {
                                if(earTremorState == RUNNING) Thread::Sleep(100);
                                else break;
                        }

                        int ear = rnd->Next(2);
                        int count = rnd->Next(3)+1;
                        int direction = rnd->Next(2);

                        if (ear == LEFT) ear = ear_l;
                        else ear = ear_r; 


                        for(int i=0; i<count; i++)
                        {
                                if(direction == 1)
                                {
                                        //nach unten zucken
                                        int diff = (servos[ear]->startValue - servos[ear]->minValue) / count;
                                        setServoValue(ear, 0, servos[ear]->startValue-diff);
                                        Threading::Thread::Sleep(200);
                                        setServoValue(ear, 0, servos[ear]->startValue);
                                }
                                else
                                {
                                        //nach oben zucken
                                        int diff = (servos[ear]->maxValue - servos[ear]->startValue) / count;
                                        setServoValue(ear, 0, servos[ear]->startValue+diff);
                                        Threading::Thread::Sleep(200);
                                        setServoValue(ear, 0, servos[ear]->startValue);
                                }
                                if (count>1) Threading::Thread::Sleep(200);
                        }

                }
                Thread::Sleep(100);
        }
}

void RobotControl::blink(void)
{       
        long sleepTime;
        Random^ rnd = gcnew Random();

        while(blinkingState != STOPPED)
        {
                while(blinkingState == RUNNING)
                {
                        sleepTime = rnd->Next(20000, 30000);
                        for(int i=0; i< (sleepTime/100); i++)
                        {
                                if(blinkingState == RUNNING) Thread::Sleep(100);
                                else break;
                        }

                        setEyeOpen(LEFT, false);
                        Threading::Thread::Sleep(50);
                        setEyeOpen(RIGHT, false);

                        Threading::Thread::Sleep(200);

                        setEyeOpen(LEFT, true);
                        Threading::Thread::Sleep(50);
                        setEyeOpen(RIGHT, true);
                }
                Thread::Sleep(100);
        }
}

void RobotControl::setServosEnable(bool enabled)
{
        try
        {
                if (enabled) serial_Port->WriteLine("!ENABLE");
                else serial_Port->WriteLine("!DISABLE");
        }
        catch(Exception^) {}
}

void RobotControl::showDisplay(String^ text)
{
        try
        {
                serial_Port->WriteLine(text);   
        }
        catch(Exception^) {};
}

void RobotControl::speechVisemeEvent(int streamNumber, System::Object^ streamPosition, int duration, SpeechLib::SpeechVisemeType nextVisemeID, SpeechLib::SpeechVisemeFeature feature, SpeechLib::SpeechVisemeType currentVisemeId)
{
        if(isMouthMovement)
        {
                int visemeType = (int)currentVisemeId;

                if (visemeType > 0)
                {
                        if (true) //(visemeCount >= 0)
                        {
                                visemeCount = 0;

                                if (visemeType == 21)   //m b p
                                {
                                        setServoValue(mouth, 0, 1368);
                                        setServoValue(jaw,   0, 1412);
                                }
                                else if ((visemeType == 2) || (visemeType == 3) || (visemeType == 8)) //aa ao ow
                                {
                                        setServoValue(mouth, 0, 1344);
                                        setServoValue(jaw,   0, 1678);
                                }
                                else if ((visemeType == 1) || (visemeType == 4) || (visemeType == 11) || (visemeType == 12)) //ih ae ah ay h
                                {
                                        setServoValue(mouth, 0, 1456);
                                        setServoValue(jaw,   0, 1612);
                                }
                                else if (visemeType == 9) // aw
                                {
                                        setServoValue(mouth, 0, 1122);
                                        setServoValue(jaw,   0, 1678);
                                }
                                else if ((visemeType == 15) || (visemeType == 16) || (visemeType == 17) || (visemeType == 19) || (visemeType == 20)) //n ng ch dh d g f k t z zh s sh
                                {
                                        setServoValue(mouth, 0, 1522);
                                        setServoValue(jaw,   0, 1522);
                                }
                                else if (visemeType == 6) //ih ey y
                                {
                                        setServoValue(mouth, 0, 1456);
                                        setServoValue(jaw,   0, 1612);
                                }
                                else if ((visemeType == 7) || (visemeType == 10)) //uw w oy
                                {
                                        setServoValue(mouth, 0, 1412);
                                        setServoValue(jaw,   0, 1612);
                                }
                                else if (visemeType == 18) //f v
                                {
                                        setServoValue(mouth, 0, 1368);
                                        setServoValue(jaw,   0, 1434);
                                }
                                else if (visemeType == 14) //l
                                {
                                        setServoValue(mouth, 0, 1100);
                                        setServoValue(jaw,   0, 1590);
                                }
                                else if ((visemeType == 13) || (visemeType == 5)) //r er
                                {
                                        setServoValue(mouth, 0, 1100);
                                        setServoValue(jaw,   0, 1456);
                                }
                        }
                        visemeCount++;
                }
                else
                {
                        setMouthSilentViseme();
                }

        }
}


void RobotControl::setMouthSilentViseme(void)
{
        setServoValue(jaw,   0, servos[jaw]->neutralValue );
        setServoValue(mouth, 0, servos[jaw]->neutralValue );
}

SpeechLib::_ISpeechVoiceEvents_VisemeEventHandler^ RobotControl::getVisemeEventHandler(void)
{
        return spVisemeEventHandler;    
}


void RobotControl::initiateServoSettings(void)
{
        servos = gcnew array<Servo^, 1>(16);

        for(int i=0;i<16;i++)
        {
                servos[i] = gcnew Servo();
        }
        
        readServoSettings(); //from XML file

}

void RobotControl::initiateRobot(void)
{
        for(int i = 0; i< 16; i++)
        {
                System::String^ servo_name = servos[i]->servoName;

                if( servo_name->CompareTo("") != 0)
                {
                  setServoValue(i, 16, servos[i]->startValue);
                }
        }
}

array<System::String^, 1>^ RobotControl::getComPortNames(void)
{
        return serial_Port->GetPortNames();
}

System::String^ RobotControl::findComport(System::String^ start_port_for_search)
{
        array<System::String^, 1>^ portnames = RobotControl::getComPortNames();
        System::Array::Sort(portnames);
        int l = portnames->Length+1;

        array<System::String^, 1>^ searchports = gcnew array<System::String^, 1>(l);
        searchports[0] = start_port_for_search;
        for (int i=0;i<(l-1);i++)
        {
                searchports->SetValue(portnames->GetValue(i), i+1);
        }


        System::String^ pn;
        System::String^ ret = "";

        for(int i = 0; i < l; i++)
        {
                try
                {
                        serial_Port->BaudRate = 2400;
                        pn = (System::String^) searchports->GetValue(i);
                        serial_Port->PortName = pn;
                        
                        serial_port_received_data = "";
                        serial_Port->Open();
                        serial_Port->WriteLine("U!SCVER?");
                        Threading::Thread::Sleep(1500);
                        System::String^ answer = serial_port_received_data;
                        serial_Port->Close();

                        if(answer->Equals("U!SCVER?\r1.4"))
                        {
                                ret = pn;
                                break;
                        }
                }
                catch (Exception^ )
                {
                        serial_Port->Close();
                }
        }
        return ret;
}

void RobotControl::serialPort_DataReceived(System::Object^  sender, System::IO::Ports::SerialDataReceivedEventArgs^  e)
{
  try
  {
          if (serial_Port->IsOpen) serial_port_received_data += serial_Port->ReadExisting();  //serial_Port->ReadLine();
          if (serial_port_received_data->Length >= 255) serial_port_received_data = "";
  }
  catch (Exception^)
  {
          //nope!
  }
}

void RobotControl::setServoValue(int servo_nr, int servo_speed, int servo_value)
{
        if ((servo_nr >= 0)&&(servo_nr < servos->Length))
        {
                int value = servo_value / 2;
                int value_h = (value & 0xFF00) / 256;
                int value_l = (value & 0x00FF);

                System::String^ sendString = "!SC" + Char(servo_nr) + Char(servo_speed) + Char(value_l) + Char(value_h);

                try
                {
                        serial_Port->WriteLine(sendString);
                }
                catch(Exception^) {}
                servos[servo_nr]->currentValue = servo_value;
        }
}

System::String^ RobotControl::getComPortName(void)
{
        return serial_Port->PortName;
}

System::String^ RobotControl::getServoDescription(int servoNumber)
{
        if ((servoNumber >= 0) && (servoNumber < servos->Length))
        {
                return servos[servoNumber]->servoName;
        }
        else return "";
}

int RobotControl::getServoMinValue(int servoNumber)
{
        if ((servoNumber >= 0) && (servoNumber < servos->Length))
        {
                return servos[servoNumber]->minValue;
        }
        else return -1;
}

int RobotControl::getServoMaxValue(int servoNumber)
{
        if ((servoNumber >= 0) && (servoNumber < servos->Length))
        {
                return servos[servoNumber]->maxValue;
        }
        else return -1;
}

int RobotControl::getServoStartValue(int servoNumber)
{
        if ((servoNumber >= 0) && (servoNumber < servos->Length))
        {
                return servos[servoNumber]->startValue;
        }
        else return -1;
}

void RobotControl::readServoSettings(void)
{
        ear_l = -1;
        ear_r = -1;
        lid_l = -1;
        lid_r = -1;
        eye_l = -1;
        eye_r = -1;
        brows = -1;
        mouth = -1;
        jaw   = -1;
        turn  = -1;
        pitch = -1;
        
        XmlTextReader^ xmlTextReader = gcnew XmlTextReader("super-yano.xml");

        while (xmlTextReader->Read())
        {
                if (xmlTextReader->NodeType == XmlNodeType::Element)
                {
                        while (xmlTextReader->NodeType != XmlNodeType::EndElement)
                        {
                                int servo_nr = -1;
                                int start_value;
                                int min_value;
                                int max_value;
                                System::String^ servo_name;
                                if (xmlTextReader->Name->CompareTo("ear_l") == 0)
                                {
                                        servo_name = "ear_l";
                                        try
                                        {
                                                servo_nr = Convert::ToInt32( xmlTextReader->GetAttribute(0) );
                                                start_value = Convert::ToInt32( xmlTextReader->GetAttribute(1) );
                                                min_value = Convert::ToInt32( xmlTextReader->GetAttribute(2) );
                                                max_value = Convert::ToInt32( xmlTextReader->GetAttribute(3) );
                                                ear_l = servo_nr;
                                        }
                                        catch (Exception^ e)
                                        {
                                                Windows::Forms::MessageBox::Show( e->Message, "Fehler in XML-Eintrag \""+ servo_name +"\"", Windows::Forms::MessageBoxButtons::OK, Windows::Forms::MessageBoxIcon::Error);
                                        }
                                }
                                else if (xmlTextReader->Name->CompareTo("ear_r") == 0)
                                {
                                        servo_name = "ear_r";
                                        try
                                        {
                                                servo_nr = Convert::ToInt32( xmlTextReader->GetAttribute(0) );
                                                start_value = Convert::ToInt32( xmlTextReader->GetAttribute(1) );
                                                min_value = Convert::ToInt32( xmlTextReader->GetAttribute(2) );
                                                max_value = Convert::ToInt32( xmlTextReader->GetAttribute(3) );
                                                ear_r = servo_nr;
                                        }
                                        catch (Exception^ e)
                                        {
                                                Windows::Forms::MessageBox::Show( e->Message, "Fehler in XML-Eintrag \""+ servo_name +"\"", Windows::Forms::MessageBoxButtons::OK, Windows::Forms::MessageBoxIcon::Error);
                                        }
                                }
                                else if (xmlTextReader->Name->CompareTo("lid_l") == 0)
                                {
                                        servo_name = "lid_l";
                                        try
                                        {
                                                servo_nr = Convert::ToInt32( xmlTextReader->GetAttribute(0) );
                                                start_value = Convert::ToInt32( xmlTextReader->GetAttribute(1) );
                                                min_value = Convert::ToInt32( xmlTextReader->GetAttribute(2) );
                                                max_value = Convert::ToInt32( xmlTextReader->GetAttribute(3) );
                                                lid_l = servo_nr;
                                        }
                                        catch (Exception^ e)
                                        {
                                                Windows::Forms::MessageBox::Show( e->Message, "Fehler in XML-Eintrag \""+ servo_name +"\"", Windows::Forms::MessageBoxButtons::OK, Windows::Forms::MessageBoxIcon::Error);
                                        }
                                }
                                else if (xmlTextReader->Name->CompareTo("lid_r") == 0)
                                {
                                        servo_name = "lid_r";
                                        try
                                        {
                                                servo_nr = Convert::ToInt32( xmlTextReader->GetAttribute(0) );
                                                start_value = Convert::ToInt32( xmlTextReader->GetAttribute(1) );
                                                min_value = Convert::ToInt32( xmlTextReader->GetAttribute(2) );
                                                max_value = Convert::ToInt32( xmlTextReader->GetAttribute(3) );
                                                lid_r = servo_nr;
                                        }
                                        catch (Exception^ e)
                                        {
                                                Windows::Forms::MessageBox::Show( e->Message, "Fehler in XML-Eintrag \""+ servo_name +"\"", Windows::Forms::MessageBoxButtons::OK, Windows::Forms::MessageBoxIcon::Error);
                                        }
                                }
                                else if (xmlTextReader->Name->CompareTo("eye_l") == 0)
                                {
                                        servo_name = "eye_l";
                                        try
                                        {
                                                servo_nr = Convert::ToInt32( xmlTextReader->GetAttribute(0) );
                                                start_value = Convert::ToInt32( xmlTextReader->GetAttribute(1) );
                                                min_value = Convert::ToInt32( xmlTextReader->GetAttribute(2) );
                                                max_value = Convert::ToInt32( xmlTextReader->GetAttribute(3) );
                                                eye_l = servo_nr;
                                        }
                                        catch (Exception^ e)
                                        {
                                                Windows::Forms::MessageBox::Show( e->Message, "Fehler in XML-Eintrag \""+ servo_name +"\"", Windows::Forms::MessageBoxButtons::OK, Windows::Forms::MessageBoxIcon::Error);
                                        }
                                }
                                else if (xmlTextReader->Name->CompareTo("eye_r") == 0)
                                {
                                        servo_name = "eye_r";
                                        try
                                        {
                                                servo_nr = Convert::ToInt32( xmlTextReader->GetAttribute(0) );
                                                start_value = Convert::ToInt32( xmlTextReader->GetAttribute(1) );
                                                min_value = Convert::ToInt32( xmlTextReader->GetAttribute(2) );
                                                max_value = Convert::ToInt32( xmlTextReader->GetAttribute(3) );
                                                eye_r = servo_nr;
                                        }
                                        catch (Exception^ e)
                                        {
                                                Windows::Forms::MessageBox::Show( e->Message, "Fehler in XML-Eintrag \""+ servo_name +"\"", Windows::Forms::MessageBoxButtons::OK, Windows::Forms::MessageBoxIcon::Error);
                                        }
                                }
                                else if (xmlTextReader->Name->CompareTo("brows") == 0)
                                {
                                        servo_name = "brows";
                                        try
                                        {
                                                servo_nr = Convert::ToInt32( xmlTextReader->GetAttribute(0) );
                                                start_value = Convert::ToInt32( xmlTextReader->GetAttribute(1) );
                                                min_value = Convert::ToInt32( xmlTextReader->GetAttribute(2) );
                                                max_value = Convert::ToInt32( xmlTextReader->GetAttribute(3) );
                                                brows = servo_nr;
                                        }
                                        catch (Exception^ e)
                                        {
                                                Windows::Forms::MessageBox::Show( e->Message, "Fehler in XML-Eintrag \""+ servo_name +"\"", Windows::Forms::MessageBoxButtons::OK, Windows::Forms::MessageBoxIcon::Error);
                                        }
                                }
                                else if (xmlTextReader->Name->CompareTo("mouth") == 0)
                                {
                                        servo_name = "mouth";
                                        try
                                        {
                                                servo_nr = Convert::ToInt32( xmlTextReader->GetAttribute(0) );
                                                start_value = Convert::ToInt32( xmlTextReader->GetAttribute(1) );
                                                min_value = Convert::ToInt32( xmlTextReader->GetAttribute(2) );
                                                max_value = Convert::ToInt32( xmlTextReader->GetAttribute(3) );
                                                mouth = servo_nr;
                                        }
                                        catch (Exception^ e)
                                        {
                                                Windows::Forms::MessageBox::Show( e->Message, "Fehler in XML-Eintrag \""+ servo_name +"\"", Windows::Forms::MessageBoxButtons::OK, Windows::Forms::MessageBoxIcon::Error);
                                        }
                                }
                                else if (xmlTextReader->Name->CompareTo("jaw") == 0)
                                {
                                        servo_name = "jaw";
                                        try
                                        {
                                                servo_nr = Convert::ToInt32( xmlTextReader->GetAttribute(0) );
                                                start_value = Convert::ToInt32( xmlTextReader->GetAttribute(1) );
                                                min_value = Convert::ToInt32( xmlTextReader->GetAttribute(2) );
                                                max_value = Convert::ToInt32( xmlTextReader->GetAttribute(3) );
                                                jaw = servo_nr;
                                        }
                                        catch (Exception^ e)
                                        {
                                                Windows::Forms::MessageBox::Show( e->Message, "Fehler in XML-Eintrag \""+ servo_name +"\"", Windows::Forms::MessageBoxButtons::OK, Windows::Forms::MessageBoxIcon::Error);
                                        }
                                }
                                else if (xmlTextReader->Name->CompareTo("turn") == 0)
                                {
                                        servo_name = "turn";
                                        try
                                        {
                                                servo_nr = Convert::ToInt32( xmlTextReader->GetAttribute(0) );
                                                start_value = Convert::ToInt32( xmlTextReader->GetAttribute(1) );
                                                min_value = Convert::ToInt32( xmlTextReader->GetAttribute(2) );
                                                max_value = Convert::ToInt32( xmlTextReader->GetAttribute(3) );
                                                turn = servo_nr;
                                        }
                                        catch (Exception^ e)
                                        {
                                                Windows::Forms::MessageBox::Show( e->Message, "Fehler in XML-Eintrag \""+ servo_name +"\"", Windows::Forms::MessageBoxButtons::OK, Windows::Forms::MessageBoxIcon::Error);
                                        }
                                }
                                else if (xmlTextReader->Name->CompareTo("pitch") == 0)
                                {
                                        servo_name = "pitch";
                                        try
                                        {
                                                servo_nr = Convert::ToInt32( xmlTextReader->GetAttribute(0) );
                                                start_value = Convert::ToInt32( xmlTextReader->GetAttribute(1) );
                                                min_value = Convert::ToInt32( xmlTextReader->GetAttribute(2) );
                                                max_value = Convert::ToInt32( xmlTextReader->GetAttribute(3) );
                                                pitch = servo_nr;
                                        }
                                        catch (Exception^ e)
                                        {
                                                Windows::Forms::MessageBox::Show( e->Message, "Fehler in XML-Eintrag \""+ servo_name +"\"", Windows::Forms::MessageBoxButtons::OK, Windows::Forms::MessageBoxIcon::Error);
                                        }
                                }

                                if (servo_nr > -1)
                                {
                                        servos[servo_nr]->servoName = servo_name;
                                        servos[servo_nr]->minValue = min_value;
                                        servos[servo_nr]->maxValue = max_value;
                                        servos[servo_nr]->startValue = start_value;
                                        servos[servo_nr]->currentValue = start_value;
                                        servos[servo_nr]->neutralValue = start_value;
                                }

                                xmlTextReader->Read();
                        }
                }
        }
        xmlTextReader->Close();
}

---