Servo bewegt sich nicht

Hallo,
Mithilfe von Dirks Servo Base Bibliothek und dem TV_Remote Bspl habe ich folgendes Programm "geschrieben" :

Code:

---

// written David Smutny
// RP6BaseServoLib was written by Dirk
// ------------------------------------------------------------------------------------------

#include "RP6BaseServoLib.h"
#include "RP6RobotBaseLib.h"



#define RC_YOUR_OWN

#ifdef RC_YOUR_OWN
        #define RC5_KEY_LEFT                                20       
        #define RC5_KEY_RIGHT                                22               
        #define RC5_KEY_FORWARDS                        18         
        #define RC5_KEY_BACKWARDS                        24       
        #define RC5_KEY_STOP                                32               
        #define RC5_KEY_CURVE_LEFT                        17       
        #define RC5_KEY_CURVE_RIGHT                19       
        #define RC5_KEY_CURVE_BACK_LEFT        23       
        #define RC5_KEY_CURVE_BACK_RIGHT        25       
        #define RC5_KEY_LEFT_MOTOR_FWD                26
        #define RC5_KEY_LEFT_MOTOR_BWD                41
        #define RC5_KEY_RIGHT_MOTOR_FWD        42
        #define RC5_KEY_RIGHT_MOTOR_BWD        49
        #define RC5_KEY_SERVO_L            37
        #define RC5_KEY_SERVO_R                                36
#endif


#define MAX_SPEED_MOVE 200
#define MAX_SPEED_TURN 100

#define MAX_SPEED_CURVE 120
#define MAX_SPEED_CURVE2 40
#define ACCELERATE_CURVE 10
#define ACCELERATE_CURVE2 4
#define DECELERATE_CURVE 4
#define DECELERATE_CURVE2 2

#define MAX_SPEED_1_MOTOR 120

#define ACCELERATE_VALUE 8
#define DECELERATE_VALUE 4

uint8_t max_speed_left;
uint8_t max_speed_right;
uint8_t acl_left;
uint8_t acl_right;
uint8_t decl_left;
uint8_t decl_right;
uint16_t pos = 0;
uint8_t servorcan = 1;      //rechter Servo
uint8_t servolcan = 1;                //linker Ser

void setDefaultSpeedParameters(void)
{
        max_speed_left = MAX_SPEED_MOVE;
        max_speed_right = max_speed_left;
        acl_left = ACCELERATE_VALUE;
        acl_right = ACCELERATE_VALUE;
        decl_left = DECELERATE_VALUE;
        decl_right = DECELERATE_VALUE;
        uint16_t tmp = (getDesSpeedLeft() + getDesSpeedRight())/2;
        moveAtSpeed(tmp , tmp);
}


void receiveRC5Data(RC5data_t rc5data)
{
        writeString_P("Toggle Bit:");
        writeChar(rc5data.toggle_bit + '0');
        writeString_P(" | Device Address:");
        writeInteger(rc5data.device, DEC);
        writeString_P(" | Key Code:");
        writeInteger(rc5data.key_code, DEC);
        writeChar('\n');
       
#ifndef DO_NOT_MOVE 
       
        uint8_t movement_command = false;

        switch(rc5data.key_code)
        {
                case RC5_KEY_LEFT:       
                        writeString_P("LEFT\n");
                        setDefaultSpeedParameters();
                        max_speed_left = MAX_SPEED_TURN;
                        max_speed_right = max_speed_left;
                        changeDirection(LEFT);
                        setLEDs(0b100000);
                        movement_command = true;
                break;
                case RC5_KEY_RIGHT:
                        writeString_P("RIGHT\n");
                        setDefaultSpeedParameters();
                        max_speed_left = MAX_SPEED_TURN;
                        max_speed_right = max_speed_left;
                        changeDirection(RIGHT);
                        setLEDs(0b000100);
                        movement_command = true;
                break;
                case RC5_KEY_FORWARDS:
                        writeString_P("FORWARDS\n");
                        setDefaultSpeedParameters();
                        changeDirection(FWD);
                        setLEDs(0b100100);
                        movement_command = true;
                break;
                case RC5_KEY_BACKWARDS:
                        writeString_P("BACKWARDS\n");
                        setDefaultSpeedParameters();
                        changeDirection(BWD);
                        setLEDs(0b001001);
                        movement_command = true;
                break;
                case RC5_KEY_STOP:
                        writeString_P("STOP\n");
                        max_speed_left = 0;
                        max_speed_right = max_speed_left;
                        moveAtSpeed(0,0);
                        setLEDs(0b011011);
                        movement_command = true;
                break;
                case RC5_KEY_CURVE_LEFT:
                        writeString_P("CURVE LEFT FWD\n");
                        max_speed_left = MAX_SPEED_CURVE2;
                        max_speed_right = MAX_SPEED_CURVE;
                        acl_left = ACCELERATE_CURVE2;
                        acl_right = ACCELERATE_CURVE;
                        decl_left = DECELERATE_CURVE2;
                        decl_right = DECELERATE_CURVE;
                        changeDirection(FWD);
                        setLEDs(0b110100);
                        movement_command = true;
                break;
                case RC5_KEY_CURVE_RIGHT:
                        writeString_P("CURVE RIGHT FWD\n");
                        max_speed_left = MAX_SPEED_CURVE;
                        max_speed_right = MAX_SPEED_CURVE2;
                        acl_left = ACCELERATE_CURVE;
                        acl_right = ACCELERATE_CURVE2;
                        decl_left = DECELERATE_CURVE;
                        decl_right = DECELERATE_CURVE2;
                        changeDirection(FWD);
                        setLEDs(0b100110);
                        movement_command = true;
                break;
                case RC5_KEY_CURVE_BACK_LEFT:
                        writeString_P("CURVE LEFT BWD\n");
                        max_speed_left = MAX_SPEED_CURVE2;
                        max_speed_right = MAX_SPEED_CURVE;
                        acl_left = ACCELERATE_CURVE2;
                        acl_right = ACCELERATE_CURVE;
                        decl_left = DECELERATE_CURVE2;
                        decl_right = DECELERATE_CURVE;
                        changeDirection(BWD);
                        setLEDs(0b011001);
                        movement_command = true;
                break;
                case RC5_KEY_CURVE_BACK_RIGHT:
                        writeString_P("CURVE RIGHT BWD\n");
                        max_speed_left = MAX_SPEED_CURVE;
                        max_speed_right = MAX_SPEED_CURVE2;
                        acl_left = ACCELERATE_CURVE;
                        acl_right = ACCELERATE_CURVE2;
                        decl_left = DECELERATE_CURVE;
                        decl_right = DECELERATE_CURVE2;
                        changeDirection(BWD);
                        setLEDs(0b001011);
                        movement_command = true;
                break;
                case RC5_KEY_LEFT_MOTOR_FWD:
                        writeString_P("MOTOR LEFT FWD\n");
                        max_speed_left = 0;
                        max_speed_right = MAX_SPEED_1_MOTOR;
                        acl_left = 4;
                        acl_right = 4;
                        decl_left = 4;
                        decl_right = 4;
                        changeDirection(FWD);
                        setLEDs(0b110000);
                        movement_command = true;
                break;
                case RC5_KEY_LEFT_MOTOR_BWD:
                        writeString_P("MOTOR LEFT BWD\n");
                        max_speed_left = 0;
                        max_speed_right = MAX_SPEED_1_MOTOR;
                        acl_left = 4;
                        acl_right = 4;
                        decl_left = 4;
                        decl_right = 4;
                        changeDirection(BWD);
                        setLEDs(0b101000);
                        movement_command = true;
                break;
                case RC5_KEY_RIGHT_MOTOR_FWD:
                        writeString_P("MOTOR RIGHT FWD\n");
                        max_speed_left = MAX_SPEED_1_MOTOR;
                        max_speed_right = 0;
                        acl_left = 4;
                        acl_right = 4;
                        decl_left = 4;
                        decl_right = 4;
                        changeDirection(FWD);
                        setLEDs(0b000110);
                        movement_command = true;
                break;
                case RC5_KEY_RIGHT_MOTOR_BWD:
                        writeString_P("MOTOR RIGHT BWD\n");
                        max_speed_left = MAX_SPEED_1_MOTOR;
                        max_speed_right = 0;
                        acl_left = 4;
                        acl_right = 4;
                        decl_left = 4;
                        decl_right = 4;
                        changeDirection(BWD);
                        setLEDs(0b000101);
                        movement_command = true;
                break;
               
                case RC5_KEY_SERVO_L:
                        writeString_P("Servo nach links\n");
                       
                                pos -= 10;
                               
                break;
               
                case RC5_KEY_SERVO_R:
                        writeString_P("Servo nach rechts\n");
                       
                                pos += 10;
                               
                break;
        }
       
        if(movement_command)
        {
                if(getDesSpeedLeft() < max_speed_left)
                {                                                       
                        moveAtSpeed(getDesSpeedLeft()+acl_left, getDesSpeedRight());
                        if(getDesSpeedLeft() < 10)
                                moveAtSpeed(10, getDesSpeedRight());
                }
                if(getDesSpeedRight() < max_speed_right)
                {
                        moveAtSpeed(getDesSpeedLeft(), getDesSpeedRight()+acl_right);
                        if(getDesSpeedRight() < 10)
                                moveAtSpeed(getDesSpeedLeft(), 10);
                }
                setStopwatch4(0);
                startStopwatch4();
        }
#endif

}


void deccelerate(void)
{
        if(getStopwatch4() > 250)
        {
                if(getDesSpeedLeft() <= 10)
                        moveAtSpeed(0, getDesSpeedRight());
                else                       
                        moveAtSpeed(getDesSpeedLeft()-decl_left, getDesSpeedRight());
                if(getDesSpeedRight() <= 10)
                        moveAtSpeed(getDesSpeedLeft(), 0);
                else               
                        moveAtSpeed(getDesSpeedLeft(), getDesSpeedRight()-decl_right);
                       
                if (getDesSpeedRight() == 0 && getDesSpeedLeft() == 0)       
                        stopStopwatch1();
                max_speed_left = getDesSpeedLeft();
                max_speed_right = getDesSpeedRight();
                setLEDs(0b000000);
                setStopwatch4(0);
        }
       
        if(getDesSpeedLeft() > max_speed_left)
        {
                if(getDesSpeedLeft() <= 10)
                        moveAtSpeed(0, getDesSpeedRight());
                else
                        moveAtSpeed(getDesSpeedLeft()-decl_left, getDesSpeedRight());
        }
        if(getDesSpeedRight() > max_speed_right)
        {
                if(getDesSpeedRight() <= 10)
                                moveAtSpeed(getDesSpeedLeft(), 0);
                else
                        moveAtSpeed(getDesSpeedLeft(), getDesSpeedRight()-decl_right);
        }
}




int main(void)
{
        initRobotBase();
       
        setLEDs(0b111111);
        writeChar('\n');
    writeString_P("RP6 controlled by RC5 TV Remote\n");
        writeString_P("___________________________\n");
        mSleep(500);       
        setLEDs(0b000000);
        powerON();
       
        IRCOMM_setRC5DataReadyHandler(receiveRC5Data);

        writeString_P("\n * Turn Left: ");                                        writeInteger(RC5_KEY_LEFT, DEC);
        writeString_P("\n * Turn Right: ");                                writeInteger(RC5_KEY_RIGHT, DEC);
        writeString_P("\n * Move Forwards: ");                                writeInteger(RC5_KEY_FORWARDS, DEC);
        writeString_P("\n * Move Backwards: ");                        writeInteger(RC5_KEY_BACKWARDS, DEC);
        writeString_P("\n * Stop: ");                                                writeInteger(RC5_KEY_STOP, DEC);
        writeString_P("\n * Move curve left forwards: ");        writeInteger(RC5_KEY_CURVE_LEFT, DEC);
        writeString_P("\n * Move curve right forwards: ");        writeInteger(RC5_KEY_CURVE_RIGHT, DEC);
        writeString_P("\n * Move curve left backwards: ");        writeInteger(RC5_KEY_CURVE_BACK_LEFT, DEC);
        writeString_P("\n * Move curve right backwards: "); writeInteger(RC5_KEY_CURVE_BACK_RIGHT, DEC);
        writeString_P("\n * Motor left forwards: ");                writeInteger(RC5_KEY_LEFT_MOTOR_FWD, DEC);
        writeString_P("\n * Motor left backwards: ");                writeInteger(RC5_KEY_LEFT_MOTOR_BWD, DEC);
        writeString_P("\n * Motor right forwards: ");                writeInteger(RC5_KEY_RIGHT_MOTOR_FWD, DEC);
        writeString_P("\n * Motor right backwards: ");                writeInteger(RC5_KEY_RIGHT_MOTOR_BWD, DEC);
        writeString_P("\n * Servo nach rechts: ");                        writeInteger(RC5_KEY_SERVO_R, DEC);
        writeString_P("\n * Servo nach links: ");                        writeInteger(RC5_KEY_SERVO_L, DEC);
        writeString_P("\n-----------------------\n");
       
       
        startStopwatch2();
       
       
        initSERVO(SERVO1);
       
        while(true)
        { 
       
                servo1_position = pos;
               
               
                deccelerate();
                task_RP6System();
               
               
        }
        return 0;
}

---

Code:

---


Wenn ich nun die entsprechende Taste drücke bewegt sich der servo aber nicht...  :-s
Kann mir jemand helfen?

danke schonmal

gruß
David

---

Hallo David,

in die while(true) Schleife müßtest du noch die task_SERVO aufnehmen und sagen, wie sich das Servo bewegen soll:

Code:

---

        startStopwatch2();                                        // Used for the demo
       
        while(true) 
        {
// ---------------------------------------------------------------------
// The demo code for positioning servo 1:
                if (getStopwatch2() > 48) {        // Change position every ~50ms
                        servo1_position = pos;                // Position of servo 1
                        pos++;                                                // Next position to the right
                        if (pos > RIGHT_TOUCH) {pos = 0;} // pos: 0..RIGHT_TOUCH
                        setStopwatch2(0);
                }
// ---------------------------------------------------------------------

                task_SERVO();                             
                deccelerate();                             
                task_RP6System();                             
        }

---

Gruß Dirk

hallo dirk,
danke für deine antwort, ich werds gleich mal ausprobieren =)
david

Hallo Dirk,

Ich habs jetzt so verändert wie du´s gesagt hast...

Code:

---

// written by David Smutny
// RP6BaseServoLib was written by Dirk
// ------------------------------------------------------------------------------------------

#include "RP6BaseServoLib.h"
#include "RP6RobotBaseLib.h"



#define RC_YOUR_OWN

#ifdef RC_YOUR_OWN
        #define RC5_KEY_LEFT                                20       
        #define RC5_KEY_RIGHT                                22               
        #define RC5_KEY_FORWARDS                        18         
        #define RC5_KEY_BACKWARDS                        24       
        #define RC5_KEY_STOP                                32               
        #define RC5_KEY_CURVE_LEFT                        17       
        #define RC5_KEY_CURVE_RIGHT                19       
        #define RC5_KEY_CURVE_BACK_LEFT        23       
        #define RC5_KEY_CURVE_BACK_RIGHT        25       
        #define RC5_KEY_LEFT_MOTOR_FWD                26
        #define RC5_KEY_LEFT_MOTOR_BWD                41
        #define RC5_KEY_RIGHT_MOTOR_FWD        42
        #define RC5_KEY_RIGHT_MOTOR_BWD        49
        #define RC5_KEY_SERVO_L            37
        #define RC5_KEY_SERVO_R                                36
#endif


#define MAX_SPEED_MOVE 200
#define MAX_SPEED_TURN 100

#define MAX_SPEED_CURVE 120
#define MAX_SPEED_CURVE2 40
#define ACCELERATE_CURVE 10
#define ACCELERATE_CURVE2 4
#define DECELERATE_CURVE 4
#define DECELERATE_CURVE2 2

#define MAX_SPEED_1_MOTOR 120

#define ACCELERATE_VALUE 8
#define DECELERATE_VALUE 4

uint8_t max_speed_left;
uint8_t max_speed_right;
uint8_t acl_left;
uint8_t acl_right;
uint8_t decl_left;
uint8_t decl_right;
uint16_t pos = 100;

void setDefaultSpeedParameters(void)
{
        max_speed_left = MAX_SPEED_MOVE;
        max_speed_right = max_speed_left;
        acl_left = ACCELERATE_VALUE;
        acl_right = ACCELERATE_VALUE;
        decl_left = DECELERATE_VALUE;
        decl_right = DECELERATE_VALUE;
        uint16_t tmp = (getDesSpeedLeft() + getDesSpeedRight())/2;
        moveAtSpeed(tmp , tmp);
}


void receiveRC5Data(RC5data_t rc5data)
{
        writeString_P("Toggle Bit:");
        writeChar(rc5data.toggle_bit + '0');
        writeString_P(" | Device Address:");
        writeInteger(rc5data.device, DEC);
        writeString_P(" | Key Code:");
        writeInteger(rc5data.key_code, DEC);
        writeChar('\n');
       
#ifndef DO_NOT_MOVE 
       
        uint8_t movement_command = false;

        switch(rc5data.key_code)
        {
                case RC5_KEY_LEFT:       
                        writeString_P("LEFT\n");
                        setDefaultSpeedParameters();
                        max_speed_left = MAX_SPEED_TURN;
                        max_speed_right = max_speed_left;
                        changeDirection(LEFT);
                        setLEDs(0b100000);
                        movement_command = true;
                break;
                case RC5_KEY_RIGHT:
                        writeString_P("RIGHT\n");
                        setDefaultSpeedParameters();
                        max_speed_left = MAX_SPEED_TURN;
                        max_speed_right = max_speed_left;
                        changeDirection(RIGHT);
                        setLEDs(0b000100);
                        movement_command = true;
                break;
                case RC5_KEY_FORWARDS:
                        writeString_P("FORWARDS\n");
                        setDefaultSpeedParameters();
                        changeDirection(FWD);
                        setLEDs(0b100100);
                        movement_command = true;
                break;
                case RC5_KEY_BACKWARDS:
                        writeString_P("BACKWARDS\n");
                        setDefaultSpeedParameters();
                        changeDirection(BWD);
                        setLEDs(0b001001);
                        movement_command = true;
                break;
                case RC5_KEY_STOP:
                        writeString_P("STOP\n");
                        max_speed_left = 0;
                        max_speed_right = max_speed_left;
                        moveAtSpeed(0,0);
                        setLEDs(0b011011);
                        movement_command = true;
                break;
                case RC5_KEY_CURVE_LEFT:
                        writeString_P("CURVE LEFT FWD\n");
                        max_speed_left = MAX_SPEED_CURVE2;
                        max_speed_right = MAX_SPEED_CURVE;
                        acl_left = ACCELERATE_CURVE2;
                        acl_right = ACCELERATE_CURVE;
                        decl_left = DECELERATE_CURVE2;
                        decl_right = DECELERATE_CURVE;
                        changeDirection(FWD);
                        setLEDs(0b110100);
                        movement_command = true;
                break;
                case RC5_KEY_CURVE_RIGHT:
                        writeString_P("CURVE RIGHT FWD\n");
                        max_speed_left = MAX_SPEED_CURVE;
                        max_speed_right = MAX_SPEED_CURVE2;
                        acl_left = ACCELERATE_CURVE;
                        acl_right = ACCELERATE_CURVE2;
                        decl_left = DECELERATE_CURVE;
                        decl_right = DECELERATE_CURVE2;
                        changeDirection(FWD);
                        setLEDs(0b100110);
                        movement_command = true;
                break;
                case RC5_KEY_CURVE_BACK_LEFT:
                        writeString_P("CURVE LEFT BWD\n");
                        max_speed_left = MAX_SPEED_CURVE2;
                        max_speed_right = MAX_SPEED_CURVE;
                        acl_left = ACCELERATE_CURVE2;
                        acl_right = ACCELERATE_CURVE;
                        decl_left = DECELERATE_CURVE2;
                        decl_right = DECELERATE_CURVE;
                        changeDirection(BWD);
                        setLEDs(0b011001);
                        movement_command = true;
                break;
                case RC5_KEY_CURVE_BACK_RIGHT:
                        writeString_P("CURVE RIGHT BWD\n");
                        max_speed_left = MAX_SPEED_CURVE;
                        max_speed_right = MAX_SPEED_CURVE2;
                        acl_left = ACCELERATE_CURVE;
                        acl_right = ACCELERATE_CURVE2;
                        decl_left = DECELERATE_CURVE;
                        decl_right = DECELERATE_CURVE2;
                        changeDirection(BWD);
                        setLEDs(0b001011);
                        movement_command = true;
                break;
                case RC5_KEY_LEFT_MOTOR_FWD:
                        writeString_P("MOTOR LEFT FWD\n");
                        max_speed_left = 0;
                        max_speed_right = MAX_SPEED_1_MOTOR;
                        acl_left = 4;
                        acl_right = 4;
                        decl_left = 4;
                        decl_right = 4;
                        changeDirection(FWD);
                        setLEDs(0b110000);
                        movement_command = true;
                break;
                case RC5_KEY_LEFT_MOTOR_BWD:
                        writeString_P("MOTOR LEFT BWD\n");
                        max_speed_left = 0;
                        max_speed_right = MAX_SPEED_1_MOTOR;
                        acl_left = 4;
                        acl_right = 4;
                        decl_left = 4;
                        decl_right = 4;
                        changeDirection(BWD);
                        setLEDs(0b101000);
                        movement_command = true;
                break;
                case RC5_KEY_RIGHT_MOTOR_FWD:
                        writeString_P("MOTOR RIGHT FWD\n");
                        max_speed_left = MAX_SPEED_1_MOTOR;
                        max_speed_right = 0;
                        acl_left = 4;
                        acl_right = 4;
                        decl_left = 4;
                        decl_right = 4;
                        changeDirection(FWD);
                        setLEDs(0b000110);
                        movement_command = true;
                break;
                case RC5_KEY_RIGHT_MOTOR_BWD:
                        writeString_P("MOTOR RIGHT BWD\n");
                        max_speed_left = MAX_SPEED_1_MOTOR;
                        max_speed_right = 0;
                        acl_left = 4;
                        acl_right = 4;
                        decl_left = 4;
                        decl_right = 4;
                        changeDirection(BWD);
                        setLEDs(0b000101);
                        movement_command = true;
                break;
               
                case RC5_KEY_SERVO_L:
                        writeString_P("Servo nach links\n");
                        if (pos > RIGHT_TOUCH)
                                pos ++;
                                writeInteger(pos, DEC);
                break;
               
                case RC5_KEY_SERVO_R:
                        writeString_P("Servo nach rechts\n");
                        if (pos < LEFT_TOUCH)
                                pos --;
                                writeInteger(pos, DEC);
                break;
        }
       
        if(movement_command)
        {
                if(getDesSpeedLeft() < max_speed_left)
                {                                                       
                        moveAtSpeed(getDesSpeedLeft()+acl_left, getDesSpeedRight());
                        if(getDesSpeedLeft() < 10)
                                moveAtSpeed(10, getDesSpeedRight());
                }
                if(getDesSpeedRight() < max_speed_right)
                {
                        moveAtSpeed(getDesSpeedLeft(), getDesSpeedRight()+acl_right);
                        if(getDesSpeedRight() < 10)
                                moveAtSpeed(getDesSpeedLeft(), 10);
                }
                setStopwatch4(0);
                startStopwatch4();
        }
#endif

}


void deccelerate(void)
{
        if(getStopwatch4() > 250)
        {
                if(getDesSpeedLeft() <= 10)
                        moveAtSpeed(0, getDesSpeedRight());
                else                       
                        moveAtSpeed(getDesSpeedLeft()-decl_left, getDesSpeedRight());
                if(getDesSpeedRight() <= 10)
                        moveAtSpeed(getDesSpeedLeft(), 0);
                else               
                        moveAtSpeed(getDesSpeedLeft(), getDesSpeedRight()-decl_right);
                       
                if (getDesSpeedRight() == 0 && getDesSpeedLeft() == 0)       
                        stopStopwatch1();
                max_speed_left = getDesSpeedLeft();
                max_speed_right = getDesSpeedRight();
                setLEDs(0b000000);
                setStopwatch4(0);
        }
       
        if(getDesSpeedLeft() > max_speed_left)
        {
                if(getDesSpeedLeft() <= 10)
                        moveAtSpeed(0, getDesSpeedRight());
                else
                        moveAtSpeed(getDesSpeedLeft()-decl_left, getDesSpeedRight());
        }
        if(getDesSpeedRight() > max_speed_right)
        {
                if(getDesSpeedRight() <= 10)
                                moveAtSpeed(getDesSpeedLeft(), 0);
                else
                        moveAtSpeed(getDesSpeedLeft(), getDesSpeedRight()-decl_right);
        }
}




int main(void)
{
        initRobotBase();
       
        setLEDs(0b111111);
        writeChar('\n');
    writeString_P("RP6 controlled by RC5 TV Remote\n");
        writeString_P("___________________________\n");
        mSleep(500);       
        setLEDs(0b000000);
        powerON();
       
        IRCOMM_setRC5DataReadyHandler(receiveRC5Data);

        writeString_P("\n * Turn Left: ");                                        writeInteger(RC5_KEY_LEFT, DEC);
        writeString_P("\n * Turn Right: ");                                writeInteger(RC5_KEY_RIGHT, DEC);
        writeString_P("\n * Move Forwards: ");                                writeInteger(RC5_KEY_FORWARDS, DEC);
        writeString_P("\n * Move Backwards: ");                        writeInteger(RC5_KEY_BACKWARDS, DEC);
        writeString_P("\n * Stop: ");                                                writeInteger(RC5_KEY_STOP, DEC);
        writeString_P("\n * Move curve left forwards: ");        writeInteger(RC5_KEY_CURVE_LEFT, DEC);
        writeString_P("\n * Move curve right forwards: ");        writeInteger(RC5_KEY_CURVE_RIGHT, DEC);
        writeString_P("\n * Move curve left backwards: ");        writeInteger(RC5_KEY_CURVE_BACK_LEFT, DEC);
        writeString_P("\n * Move curve right backwards: "); writeInteger(RC5_KEY_CURVE_BACK_RIGHT, DEC);
        writeString_P("\n * Motor left forwards: ");                writeInteger(RC5_KEY_LEFT_MOTOR_FWD, DEC);
        writeString_P("\n * Motor left backwards: ");                writeInteger(RC5_KEY_LEFT_MOTOR_BWD, DEC);
        writeString_P("\n * Motor right forwards: ");                writeInteger(RC5_KEY_RIGHT_MOTOR_FWD, DEC);
        writeString_P("\n * Motor right backwards: ");                writeInteger(RC5_KEY_RIGHT_MOTOR_BWD, DEC);
        writeString_P("\n * Servo nach rechts: ");                        writeInteger(RC5_KEY_SERVO_R, DEC);
        writeString_P("\n * Servo nach links: ");                        writeInteger(RC5_KEY_SERVO_L, DEC);
        writeString_P("\n-----------------------\n");
       
       
        startStopwatch2();
       
       
        initSERVO(SERVO1);
       
        while(true)
       
  {
      if (getStopwatch2() > 48)
       
                {servo1_position = pos;}
       
               
                setStopwatch2(0);
               
                deccelerate();
                task_SERVO();
                task_RP6System();
               
               
        }
        return 0;
}

---

Der Servo will aber noch immer nicht...
[-(

Was ist noch falsch??
Bei deinem Beispiel-Programm bewagt er sich...

gruß David

Hallo David,

erstmal die Klammern anders:

Code:

---

  startStopwatch2();
   
   
  initSERVO(SERVO1);
   
  while(true)
   
  {
      if (getStopwatch2() > 48)
         
      {servo1_position = pos;
         
     
      setStopwatch2(0);
      }
 
      deccelerate();
      task_SERVO();
      task_RP6System();
  }     
     
  return 0;
}

---

... und dann wird sich das Servo ja nur bewegen, wenn sich pos irgendwie ändert. Das must du machen.

Gruß Dirk

Ja, genau...
Das mach ich doch mit:

Code:

---

               
                case RC5_KEY_SERVO_L:
                        writeString_P("Servo nach links\n");
                        if (pos > RIGHT_TOUCH)
                                pos ++;
                                writeString_P("Servo Position:\n");  writeInteger(pos, DEC);
                break;
               
                case RC5_KEY_SERVO_R:
                        writeString_P("Servo nach rechts\n");
                        if (pos < LEFT_TOUCH)
                                pos --;
                                writeString_P("Servo Position:\n");  writeInteger(pos, DEC);
                break;
        }

---

oder?
Also leider gehts bei mir noch immer nicht... :-k

Wie ich es drehe und wende... Es will nicht funktionieren =(

Poste doch deinen aktuellen Code noch 'mal.

Ich probiere ihn dann mal bei mir.

Gruß Dirk

Hallo Dirk,

vielen Dank für deine Hilfe.
Hier mein Aktueller Code:

Code:

---

// written by David Smutny
// RP6BaseServoLib was written by Dirk
// ------------------------------------------------------------------------------------------

#include "RP6BaseServoLib.h"
#include "RP6RobotBaseLib.h"



#define RC_YOUR_OWN

#ifdef RC_YOUR_OWN
        #define RC5_KEY_LEFT                                20       
        #define RC5_KEY_RIGHT                                22               
        #define RC5_KEY_FORWARDS                        18         
        #define RC5_KEY_BACKWARDS                        24       
        #define RC5_KEY_STOP                                32               
        #define RC5_KEY_CURVE_LEFT                        17       
        #define RC5_KEY_CURVE_RIGHT                19       
        #define RC5_KEY_CURVE_BACK_LEFT        23       
        #define RC5_KEY_CURVE_BACK_RIGHT        25       
        #define RC5_KEY_LEFT_MOTOR_FWD                26
        #define RC5_KEY_LEFT_MOTOR_BWD                41
        #define RC5_KEY_RIGHT_MOTOR_FWD        42
        #define RC5_KEY_RIGHT_MOTOR_BWD        49
        #define RC5_KEY_SERVO_L            37
        #define RC5_KEY_SERVO_R                                36
#endif


#define MAX_SPEED_MOVE 200
#define MAX_SPEED_TURN 100

#define MAX_SPEED_CURVE 120
#define MAX_SPEED_CURVE2 40
#define ACCELERATE_CURVE 10
#define ACCELERATE_CURVE2 4
#define DECELERATE_CURVE 4
#define DECELERATE_CURVE2 2

#define MAX_SPEED_1_MOTOR 120

#define ACCELERATE_VALUE 8
#define DECELERATE_VALUE 4

uint8_t max_speed_left;
uint8_t max_speed_right;
uint8_t acl_left;
uint8_t acl_right;
uint8_t decl_left;
uint8_t decl_right;
uint16_t pos = 100;

void setDefaultSpeedParameters(void)
{
        max_speed_left = MAX_SPEED_MOVE;
        max_speed_right = max_speed_left;
        acl_left = ACCELERATE_VALUE;
        acl_right = ACCELERATE_VALUE;
        decl_left = DECELERATE_VALUE;
        decl_right = DECELERATE_VALUE;
        uint16_t tmp = (getDesSpeedLeft() + getDesSpeedRight())/2;
        moveAtSpeed(tmp , tmp);
}


void receiveRC5Data(RC5data_t rc5data)
{
        writeString_P("Toggle Bit:");
        writeChar(rc5data.toggle_bit + '0');
        writeString_P(" | Device Address:");
        writeInteger(rc5data.device, DEC);
        writeString_P(" | Key Code:");
        writeInteger(rc5data.key_code, DEC);
        writeChar('\n');
       
#ifndef DO_NOT_MOVE 
       
        uint8_t movement_command = false;

        switch(rc5data.key_code)
        {
                case RC5_KEY_LEFT:       
                        writeString_P("LEFT\n");
                        setDefaultSpeedParameters();
                        max_speed_left = MAX_SPEED_TURN;
                        max_speed_right = max_speed_left;
                        changeDirection(LEFT);
                        setLEDs(0b100000);
                        movement_command = true;
                break;
                case RC5_KEY_RIGHT:
                        writeString_P("RIGHT\n");
                        setDefaultSpeedParameters();
                        max_speed_left = MAX_SPEED_TURN;
                        max_speed_right = max_speed_left;
                        changeDirection(RIGHT);
                        setLEDs(0b000100);
                        movement_command = true;
                break;
                case RC5_KEY_FORWARDS:
                        writeString_P("FORWARDS\n");
                        setDefaultSpeedParameters();
                        changeDirection(FWD);
                        setLEDs(0b100100);
                        movement_command = true;
                break;
                case RC5_KEY_BACKWARDS:
                        writeString_P("BACKWARDS\n");
                        setDefaultSpeedParameters();
                        changeDirection(BWD);
                        setLEDs(0b001001);
                        movement_command = true;
                break;
                case RC5_KEY_STOP:
                        writeString_P("STOP\n");
                        max_speed_left = 0;
                        max_speed_right = max_speed_left;
                        moveAtSpeed(0,0);
                        setLEDs(0b011011);
                        movement_command = true;
                break;
                case RC5_KEY_CURVE_LEFT:
                        writeString_P("CURVE LEFT FWD\n");
                        max_speed_left = MAX_SPEED_CURVE2;
                        max_speed_right = MAX_SPEED_CURVE;
                        acl_left = ACCELERATE_CURVE2;
                        acl_right = ACCELERATE_CURVE;
                        decl_left = DECELERATE_CURVE2;
                        decl_right = DECELERATE_CURVE;
                        changeDirection(FWD);
                        setLEDs(0b110100);
                        movement_command = true;
                break;
                case RC5_KEY_CURVE_RIGHT:
                        writeString_P("CURVE RIGHT FWD\n");
                        max_speed_left = MAX_SPEED_CURVE;
                        max_speed_right = MAX_SPEED_CURVE2;
                        acl_left = ACCELERATE_CURVE;
                        acl_right = ACCELERATE_CURVE2;
                        decl_left = DECELERATE_CURVE;
                        decl_right = DECELERATE_CURVE2;
                        changeDirection(FWD);
                        setLEDs(0b100110);
                        movement_command = true;
                break;
                case RC5_KEY_CURVE_BACK_LEFT:
                        writeString_P("CURVE LEFT BWD\n");
                        max_speed_left = MAX_SPEED_CURVE2;
                        max_speed_right = MAX_SPEED_CURVE;
                        acl_left = ACCELERATE_CURVE2;
                        acl_right = ACCELERATE_CURVE;
                        decl_left = DECELERATE_CURVE2;
                        decl_right = DECELERATE_CURVE;
                        changeDirection(BWD);
                        setLEDs(0b011001);
                        movement_command = true;
                break;
                case RC5_KEY_CURVE_BACK_RIGHT:
                        writeString_P("CURVE RIGHT BWD\n");
                        max_speed_left = MAX_SPEED_CURVE;
                        max_speed_right = MAX_SPEED_CURVE2;
                        acl_left = ACCELERATE_CURVE;
                        acl_right = ACCELERATE_CURVE2;
                        decl_left = DECELERATE_CURVE;
                        decl_right = DECELERATE_CURVE2;
                        changeDirection(BWD);
                        setLEDs(0b001011);
                        movement_command = true;
                break;
                case RC5_KEY_LEFT_MOTOR_FWD:
                        writeString_P("MOTOR LEFT FWD\n");
                        max_speed_left = 0;
                        max_speed_right = MAX_SPEED_1_MOTOR;
                        acl_left = 4;
                        acl_right = 4;
                        decl_left = 4;
                        decl_right = 4;
                        changeDirection(FWD);
                        setLEDs(0b110000);
                        movement_command = true;
                break;
                case RC5_KEY_LEFT_MOTOR_BWD:
                        writeString_P("MOTOR LEFT BWD\n");
                        max_speed_left = 0;
                        max_speed_right = MAX_SPEED_1_MOTOR;
                        acl_left = 4;
                        acl_right = 4;
                        decl_left = 4;
                        decl_right = 4;
                        changeDirection(BWD);
                        setLEDs(0b101000);
                        movement_command = true;
                break;
                case RC5_KEY_RIGHT_MOTOR_FWD:
                        writeString_P("MOTOR RIGHT FWD\n");
                        max_speed_left = MAX_SPEED_1_MOTOR;
                        max_speed_right = 0;
                        acl_left = 4;
                        acl_right = 4;
                        decl_left = 4;
                        decl_right = 4;
                        changeDirection(FWD);
                        setLEDs(0b000110);
                        movement_command = true;
                break;
                case RC5_KEY_RIGHT_MOTOR_BWD:
                        writeString_P("MOTOR RIGHT BWD\n");
                        max_speed_left = MAX_SPEED_1_MOTOR;
                        max_speed_right = 0;
                        acl_left = 4;
                        acl_right = 4;
                        decl_left = 4;
                        decl_right = 4;
                        changeDirection(BWD);
                        setLEDs(0b000101);
                        movement_command = true;
                break;
               
                case RC5_KEY_SERVO_L:
                        writeString_P("Servo nach links\n");
                        if (pos > RIGHT_TOUCH)
                                pos ++;
                                writeString_P("Servo Position:\n");  writeInteger(pos, DEC);
                break;
               
                case RC5_KEY_SERVO_R:
                        writeString_P("Servo nach rechts\n");
                        if (pos < LEFT_TOUCH)
                                pos --;
                                writeString_P("Servo Position:\n");  writeInteger(pos, DEC);
                break;
        }
       
        if(movement_command)
        {
                if(getDesSpeedLeft() < max_speed_left)
                {                                                       
                        moveAtSpeed(getDesSpeedLeft()+acl_left, getDesSpeedRight());
                        if(getDesSpeedLeft() < 10)
                                moveAtSpeed(10, getDesSpeedRight());
                }
                if(getDesSpeedRight() < max_speed_right)
                {
                        moveAtSpeed(getDesSpeedLeft(), getDesSpeedRight()+acl_right);
                        if(getDesSpeedRight() < 10)
                                moveAtSpeed(getDesSpeedLeft(), 10);
                }
                setStopwatch4(0);
                startStopwatch4();
        }
#endif

}


void deccelerate(void)
{
        if(getStopwatch4() > 250)
        {
                if(getDesSpeedLeft() <= 10)
                        moveAtSpeed(0, getDesSpeedRight());
                else                       
                        moveAtSpeed(getDesSpeedLeft()-decl_left, getDesSpeedRight());
                if(getDesSpeedRight() <= 10)
                        moveAtSpeed(getDesSpeedLeft(), 0);
                else               
                        moveAtSpeed(getDesSpeedLeft(), getDesSpeedRight()-decl_right);
                       
                if (getDesSpeedRight() == 0 && getDesSpeedLeft() == 0)       
                        stopStopwatch1();
                max_speed_left = getDesSpeedLeft();
                max_speed_right = getDesSpeedRight();
                setLEDs(0b000000);
                setStopwatch4(0);
        }
       
        if(getDesSpeedLeft() > max_speed_left)
        {
                if(getDesSpeedLeft() <= 10)
                        moveAtSpeed(0, getDesSpeedRight());
                else
                        moveAtSpeed(getDesSpeedLeft()-decl_left, getDesSpeedRight());
        }
        if(getDesSpeedRight() > max_speed_right)
        {
                if(getDesSpeedRight() <= 10)
                                moveAtSpeed(getDesSpeedLeft(), 0);
                else
                        moveAtSpeed(getDesSpeedLeft(), getDesSpeedRight()-decl_right);
        }
}




int main(void)
{
        initRobotBase();
       
        setLEDs(0b111111);
        writeChar('\n');
    writeString_P("RP6 controlled by RC5 TV Remote\n");
        writeString_P("___________________________\n");
        mSleep(500);       
        setLEDs(0b000000);
        powerON();
       
        IRCOMM_setRC5DataReadyHandler(receiveRC5Data);

        writeString_P("\n * Turn Left: ");                                        writeInteger(RC5_KEY_LEFT, DEC);
        writeString_P("\n * Turn Right: ");                                writeInteger(RC5_KEY_RIGHT, DEC);
        writeString_P("\n * Move Forwards: ");                                writeInteger(RC5_KEY_FORWARDS, DEC);
        writeString_P("\n * Move Backwards: ");                        writeInteger(RC5_KEY_BACKWARDS, DEC);
        writeString_P("\n * Stop: ");                                                writeInteger(RC5_KEY_STOP, DEC);
        writeString_P("\n * Move curve left forwards: ");        writeInteger(RC5_KEY_CURVE_LEFT, DEC);
        writeString_P("\n * Move curve right forwards: ");        writeInteger(RC5_KEY_CURVE_RIGHT, DEC);
        writeString_P("\n *curve left backwards: ");        writeInteger(RC5_KEY_CURVE_BACK_LEFT, DEC);
        writeString_P("\n * Move curve right backwards: "); writeInteger(RC5_KEY_CURVE_BACK_RIGHT, DEC);
        writeString_P("\n * Motor left forwards: ");                writeInteger(RC5_KEY_LEFT_MOTOR_FWD, DEC);
        writeString_P("\n * Motor left backwards: ");                writeInteger(RC5_KEY_LEFT_MOTOR_BWD, DEC);
        writeString_P("\n * Motor right forwards: ");                writeInteger(RC5_KEY_RIGHT_MOTOR_FWD, DEC);
        writeString_P("\n * Motor right backwards: ");                writeInteger(RC5_KEY_RIGHT_MOTOR_BWD, DEC);
        writeString_P("\n * Servo nach rechts: ");                        writeInteger(RC5_KEY_SERVO_R, DEC);
        writeString_P("\n * Servo nach links: ");                        writeInteger(RC5_KEY_SERVO_L, DEC);
        writeString_P("\n-----------------------\n");
       
  startStopwatch2();
 
 
  initSERVO(SERVO1);
 
  while(true)
 
  {
      if (getStopwatch2() > 48)
       
      {servo1_position = pos;
       
     
      setStopwatch2(0);
      }
 
      deccelerate();
      task_SERVO();
      task_RP6System();
  }     
     
  return 0;
}

---

Ich hoffe du kanst den Fehler finden!

gruß David

Hallo David,

so, nun hab ich's:

Ändere die RC5 Abfragen 'mal so:

Code:

---

      case RC5_KEY_SERVO_L:
                startSERVO();
                setStopwatch6(0);
                startStopwatch6();
                writeString_P("Servo nach links\n");
        if (pos >= 10) {
            pos -= 10;
                }
                else {
                        pos = 0;
                }
                servo1_position = pos;
        writeString_P("Servo Position:\n");  writeInteger(pos, DEC);
      break;
     
      case RC5_KEY_SERVO_R:
                startSERVO();
                setStopwatch6(0);
                startStopwatch6();
        writeString_P("Servo nach rechts\n");
        if (pos <= (RIGHT_TOUCH - 10)) {
            pos += 10;
                }
                else {
                        pos = RIGHT_TOUCH;
                }
                servo1_position = pos;
        writeString_P("Servo Position:\n");  writeInteger(pos, DEC);
      break;

---

... und die while(1) Schleife so:

Code:

---

        stopSERVO();
  while(true)
   
  {
             
       
 
      deccelerate();
      task_SERVO();
      if (getStopwatch6() > 300) {stopSERVO();}
      task_RP6System();
  }

---

Gruß Dirk