kombination von fahren(move) und servobew. (getStopwatch)
hi allerseits,
mit folgendem code versuche ich das anfahren von servopositionen mit fahren/drehen des RP6 zu kombinieren, mit mäßigem erfolg :-(
Code:
// Uncommented Version of greifarm an m32.c
// written by georg
// ------------------------------------------------------------------------------------------
#include "RP6ControlServoLib.h" //servolib von Dirk
#include "RP6I2CmasterTWI.h" // I2C Master Library
#include "RP6Control_I2CMasterLib.h" //RP6 Control I2C Master library
int pos[] = { 5, 15, 25, 35, 45, 55, 65, 75, 85, 95, 105, 115, 125};
/*****************************************************************************/
/**
* Timed Watchdog display only - the other heartbeat function
* does not work in this example as we use blocked moving functions here.
*/
void watchDogRequest(void)
{
static uint8_t heartbeat2 = false;
if(heartbeat2)
{
clearPosLCD(0, 14, 1);
heartbeat2 = false;
}
else
{
setCursorPosLCD(0, 14);
writeStringLCD_P("#");
heartbeat2 = true;
}
}
/*****************************************************************************/
// I2C Requests:
/**
* The I2C_requestedDataReady Event Handler
*/
void I2C_requestedDataReady(uint8_t dataRequestID)
{
checkRP6Status(dataRequestID);
}
/*****************************************************************************/
// I2C Error handler
/**
* This function gets called automatically if there was an I2C Error like
* the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30).
*/
void I2C_transmissionError(uint8_t errorState)
{
writeString_P("\nI2C ERROR - TWI STATE: 0x");
writeInteger(errorState, HEX);
writeChar('\n');
}
/*****************************************************************************/
/********************funktionen für servo**********************/
void servo_right_top(void)
{
servo2_position = pos[1];
servo4_position = pos[12];
}
void servo_right_bottom(void)
{
servo2_position = pos[12];
servo4_position = pos[0];
}
void servo_center_top(void)
{
servo2_position = pos[5];
servo4_position = pos[12];
}
void servo_center_bottom(void)
{
servo2_position = pos[5];
servo4_position = pos[3];
}
void servo_left_top(void)
{
servo2_position = pos[12];
servo4_position = pos[12];
}
void servo_left_bottom(void)
{
servo2_position = pos[1];
servo4_position = pos[1];
}
void servo_open(void)
{
servo6_position = pos[8];
}
void servo_close(void)
{
servo6_position = pos[4];
}
int main(void)
{
initRP6Control();
initLCD();
showScreenLCD("################", "################");
mSleep(1500);
showScreenLCD("<<RP6 Control>>", "<<LC - DISPLAY>>");
mSleep(2500);
showScreenLCD(" greifarm an m32 ", " Version 1.02 ");
mSleep(2500);
clearLCD();
initSERVO(SERVO2 | SERVO4 | SERVO6);
servo2_position = pos[5];
servo4_position = pos[5];
servo6_position = pos[5];
mSleep(2000);
startStopwatch1();
startStopwatch2();
// ---------------------------------------
WDT_setRequestHandler(watchDogRequest);
// ---------------------------------------
// Init TWI Interface:
I2CTWI_initMaster(100);
I2CTWI_setRequestedDataReadyHandler(I2C_requestedDataReady);
I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);
sound(180,80,25);
sound(220,80,25);
setLEDs(0b1111);
showScreenLCD("################", "################");
mSleep(500);
showScreenLCD("I2C-Master", "Movement...");
mSleep(1000);
setLEDs(0b0000);
// ---------------------------------------
I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_ACS_POWER, ACS_PWR_MED);
I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_WDT, true);
I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_WDT_RQ, true);
while(true)
{
if (getStopwatch2() > 1000)
{
servo_right_top();
servo_open();
}
if (getStopwatch2() > 2000)
{
servo_close();
// move(60, FWD, DIST_MM(100), true);
// setStopwatch2(0);
// startStopwatch2();
}
if (getStopwatch2() > 3000)
{
servo_left_bottom();
// rotate(50, LEFT, 180, true);
}
if (getStopwatch2() > 4000)
{
servo_left_top();
}
if (getStopwatch2() > 5000)
{
servo_right_bottom();
// move(60, FWD, DIST_MM(100), true);
}
if (getStopwatch2() > 6000)
{
servo_center_top();
}
if (getStopwatch2() > 7000)
{
servo_open();
servo_center_bottom();
setStopwatch2(0);
setStopwatch1(0);
}
task_SERVO();
mSleep(3);
// move(60, FWD, DIST_MM(100), true);
}
return 0;
}
die auskomentierten befehlszeilen in der whileschleife zeigen meine versuche, ich habe beim move true und false versucht, auch verschiedene (längere) zeitabstände bei den If getStopwatch(2) abfragen, wie auch eine zweite If getStopwatch(1) schleife innerhalb der abfrage (2) einzubauen, ohne den erwünschten erfolg...
auch wird die "zero_position" nicht mehr angefahren...
Ich möchte nur, dass der roboter ein stück fährt, dann ein, zwei servo bewegungen, dann evtl. roboter drehen, wieder servos, ...
was muss ich anders machen? Die hauptschleife in mehrere schleifen aufteilen? Oder gibts da noch ein anderen weg?...
