initialisierung, servo & SRF02

[inka]

hallo allerseits,

seit drei tagen hocke ich schon an diesem code und glaube sehe den wald vor lauter bäumen nicht

das programm (der servo soll schrittweise drehen und der SRF die entfernung messen) läuft bis auf kleinigkeiten, darum geht es hier also nicht, das sind offensichtlich noch laufzeitprobleme...

Das problem ist folgendes:

bei einschalten und starten läuft der SRF an und misst, der servo dreht aber nicht. Beim erneuten drücken des startbuttons - ohne vorher abzuschalten - läuft beides, der SRF und der servo. Wäre für mich ein initialisierungsproblem, aber wo? ich finds einfach nicht - könnte mich bitte jemand anschubsen?....

danke...

#include "RP6ControlLib.h"
#include "RP6I2CmasterTWI.h"
#include "RP6Control_MultiIOLib.h"
#include "RP6Control_I2CMasterLib.h"
#include "RP6Control_MultiIO.h"
#include "RP6Control_LFSBumperLib.h"
#include "RP6ControlServoLib.h"
#include "RP6Control_OrientationLib.h"
#include "standard.h"


#define I2C_RP6_BASE_ADR 10


uint16_t servopos;
uint8_t i;


double distsrf02_1;



/*************** hauptprogramm ***********/

int main(void)
{


    initRP6Control();
    initLCD();
    writeString_P("\n\n SRF02_plus_servo_test_3\n\n");
    writeChar('\n');

    setLEDs(0b1111);
    mSleep(500);
    setLEDs(0b0000);

    I2CTWI_initMaster(100);
    I2CTWI_setTransmissionErrorHandler(I2C_transmissionError); //aktiviert I2C fehlermeldungen

    showScreenLCD(" RP6Control M32", " SRF02_plus_servo","_test_3","");
    mSleep(1500);
    clearLCD();


    setServoPower(0);
    multiio_init();



    servopos = SERVO1_LT;


    while(true)
    {

        setServoPower(1); //doppelaufruf entfernt
        initSERVO(SERVO1 | SERVO2 | SERVO3 ); //doppelaufruf entfernt

        distsrf02_1 = SRF02_calculate(SRF02_measure(CH_SRF02_1, MODE_US));

        writeString("servopos: ");
        writeInteger(servopos, DEC);
        writeString("   distanz: ");
        writeDouble(distsrf02_1, 4, 2);
        writeString(" mm  ");
        writeString("\n");

        setServo(1, servopos);
        mSleep(600);

        servopos += 5;
        if (servopos > SERVO1_RT) servopos = SERVO1_LT;

        task_SERVO();


    }

    return 0;
}

EDIT: doppelaufrufe im code entfernt