Hier mal mein bisheriger Stand des Programms:
Leider reagiert der RP6 aber auf gar nix mehr...
Code:
// Includes:
#include "RP6RobotBaseLib.h"
#include "RP6I2CslaveTWI.h" // Include the I²C-Bus Slave Library
// The Slave Address on the I2C Bus can be specified here:
#define RP6BASE_I2C_SLAVE_ADR 10
// This bitfield contains the main interrupt event status bits. This can be
// read out and any Master devices can react on the specific events.
union {
uint8_t byte;
struct {
uint8_t batLow:1;
uint8_t bumperLeft:1;
uint8_t bumperRight:1;
uint8_t RC5reception:1;
uint8_t RC5transmitReady:1;
uint8_t obstacleLeft:1;
uint8_t obstacleRight:1;
uint8_t driveSystemChange:1;
};
} interrupt_status;
// Some status bits with current settings and other things.
union {
uint8_t byte;
struct {
uint8_t powerOn:1;
uint8_t ACSactive:1;
uint8_t watchDogTimer:1;
uint8_t wdtRequest:1;
uint8_t wdtRequestEnable:1;
uint8_t unused:3;
};
} status;
// Drive Status register contains information about current movements.
// You can check if movements are complete, if the motors are turned
// on, if there were overcurrent events and for direction.
union {
uint8_t byte;
struct {
uint8_t movementComplete:1;
uint8_t motorsOn:1;
uint8_t motorOvercurrent:1;
uint8_t direction:2;
uint8_t unused:3;
};
} drive_status;
RC5data_t lastRC5Reception;
/*****************************************************************************/
/**
* Generates Interrupt Signal and starts Software Watchdog
*/
void signalInterrupt(void)
{
I2CTWI_dataWasRead = 0;
extIntON();
if(status.watchDogTimer)
startStopwatch2();
}
/**
* Clears Interrupt
*/
void clearInterrupt(void)
{
stopStopwatch2();
setStopwatch2(0);
status.wdtRequest = false;
interrupt_status.RC5reception = false;
interrupt_status.driveSystemChange = false;
extIntOFF();
}
/**
* ACS Event Handler
*/
void acsStateChanged(void)
{
interrupt_status.obstacleLeft = obstacle_left;
interrupt_status.obstacleRight = obstacle_right;
signalInterrupt();
}
/**
* Bumpers Event Handler
*/
void bumpersStateChanged(void)
{
interrupt_status.bumperLeft = bumper_left;
if(bumper_right)
interrupt_status.bumperRight = true;
else
interrupt_status.bumperRight = false;
signalInterrupt();
}
uint16_t uBat_measure = 720;
uint8_t uBat_count = 0;
/**
* This function needs to be called frequently in the main loop. It updates
* some values (currently only Battery Voltage and Motor status, but this may
* be expanded in future).
*/
void task_update(void)
{
if(getStopwatch4() > 250)
{
uBat_measure += adcBat;
uBat_measure /= 2;
uBat_count++;
setStopwatch2(0);
}
if(uBat_count > 5)
{
if(!interrupt_status.batLow && uBat_measure < 560)
{
interrupt_status.batLow = true;
signalInterrupt();
}
else if(interrupt_status.batLow && uBat_measure > 580)
{
interrupt_status.batLow = false;
signalInterrupt();
}
uBat_count = 0;
}
drive_status.motorsOn = (mleft_power || mright_power);
drive_status.direction = getDirection();
}
/*****************************************************************************/
// I2C Registers that can be read by the Master. Their names should
// be self-explanatory and directly relate to the equivalent variables/functions
// in the RP6Library
#define I2C_REG_STATUS1 0
#define I2C_REG_STATUS2 1
#define I2C_REG_MOTION_STATUS 2
#define I2C_REG_POWER_LEFT 3
#define I2C_REG_POWER_RIGHT 4
#define I2C_REG_SPEED_LEFT 5
#define I2C_REG_SPEED_RIGHT 6
#define I2C_REG_DES_SPEED_LEFT 7
#define I2C_REG_DES_SPEED_RIGHT 8
#define I2C_REG_DIST_LEFT_L 9
#define I2C_REG_DIST_LEFT_H 10
#define I2C_REG_DIST_RIGHT_L 11
#define I2C_REG_DIST_RIGHT_H 12
#define I2C_REG_ADC_LSL_L 13
#define I2C_REG_ADC_LSL_H 14
#define I2C_REG_ADC_LSR_L 15
#define I2C_REG_ADC_LSR_H 16
#define I2C_REG_ADC_MOTOR_CURL_L 17
#define I2C_REG_ADC_MOTOR_CURL_H 18
#define I2C_REG_ADC_MOTOR_CURR_L 19
#define I2C_REG_ADC_MOTOR_CURR_H 20
#define I2C_REG_ADC_UBAT_L 21
#define I2C_REG_ADC_UBAT_H 22
#define I2C_REG_ADC_ADC0_L 23
#define I2C_REG_ADC_ADC0_H 24
#define I2C_REG_ADC_ADC1_L 25
#define I2C_REG_ADC_ADC1_H 26
#define I2C_REG_RC5_ADR 27
#define I2C_REG_RC5_DATA 28
#define I2C_REG_LEDS 29
/**
* This very important function updates ALL registers that the Master can read.
* It is called frequently out of the Main loop.
*/
void task_updateRegisters(void)
{
if(!I2CTWI_readBusy)
{
I2CTWI_readRegisters[I2C_REG_STATUS1] = (uint8_t)(interrupt_status.byte);
I2CTWI_readRegisters[I2C_REG_STATUS2] = (uint8_t)(status.byte);
I2CTWI_readRegisters[I2C_REG_MOTION_STATUS] = (uint8_t)(drive_status.byte);
I2CTWI_readRegisters[I2C_REG_POWER_LEFT] = (uint8_t)(mleft_power);
I2CTWI_readRegisters[I2C_REG_POWER_RIGHT] = (uint8_t)(mright_power);
I2CTWI_readRegisters[I2C_REG_SPEED_LEFT] = (uint8_t)(getLeftSpeed());
I2CTWI_readRegisters[I2C_REG_SPEED_RIGHT] = (uint8_t)(getRightSpeed());
I2CTWI_readRegisters[I2C_REG_DES_SPEED_LEFT] = (uint8_t)(getDesSpeedLeft());
I2CTWI_readRegisters[I2C_REG_DES_SPEED_RIGHT] = (uint8_t)(getDesSpeedRight());
I2CTWI_readRegisters[I2C_REG_DIST_LEFT_L] = (uint8_t)(getLeftDistance());
I2CTWI_readRegisters[I2C_REG_DIST_LEFT_H] = (uint8_t)(getLeftDistance()>>8);
I2CTWI_readRegisters[I2C_REG_DIST_RIGHT_L] = (uint8_t)(getRightDistance());
I2CTWI_readRegisters[I2C_REG_DIST_RIGHT_H] = (uint8_t)(getRightDistance()>>8);
I2CTWI_readRegisters[I2C_REG_ADC_LSL_L] = (uint8_t)(adcLSL);
I2CTWI_readRegisters[I2C_REG_ADC_LSL_H] = (uint8_t)(adcLSL>>8);
I2CTWI_readRegisters[I2C_REG_ADC_LSR_L] = (uint8_t)(adcLSR);
I2CTWI_readRegisters[I2C_REG_ADC_LSR_H] = (uint8_t)(adcLSR>>8);
I2CTWI_readRegisters[I2C_REG_ADC_MOTOR_CURL_L] = (uint8_t)(adcMotorCurrentLeft);
I2CTWI_readRegisters[I2C_REG_ADC_MOTOR_CURL_H] = (uint8_t)(adcMotorCurrentLeft>>8);
I2CTWI_readRegisters[I2C_REG_ADC_MOTOR_CURR_L] = (uint8_t)(adcMotorCurrentRight);
I2CTWI_readRegisters[I2C_REG_ADC_MOTOR_CURR_H] = (uint8_t)(adcMotorCurrentRight>>8);
I2CTWI_readRegisters[I2C_REG_ADC_UBAT_L] = (uint8_t)(adcBat);
I2CTWI_readRegisters[I2C_REG_ADC_UBAT_H] = (uint8_t)(adcBat>>8);
I2CTWI_readRegisters[I2C_REG_ADC_ADC0_L] = (uint8_t)(adc0);
I2CTWI_readRegisters[I2C_REG_ADC_ADC0_H] = (uint8_t)(adc0>>8);
I2CTWI_readRegisters[I2C_REG_ADC_ADC1_L] = (uint8_t)(adc1);
I2CTWI_readRegisters[I2C_REG_ADC_ADC1_H] = (uint8_t)(adc1>>8);
I2CTWI_readRegisters[I2C_REG_LEDS] = (uint8_t)(statusLEDs.byte);
I2CTWI_readRegisters[I2C_REG_RC5_ADR] = (uint8_t)((lastRC5Reception.device)|(lastRC5Reception.toggle_bit<<5));
I2CTWI_readRegisters[I2C_REG_RC5_DATA] = (uint8_t)(lastRC5Reception.key_code);
if(I2CTWI_dataWasRead && I2CTWI_dataReadFromReg == 0)
clearInterrupt();
}
}
/*****************************************************************************/
/*****************************************************************************/
// Speed values:
#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; // Maximum speed variable left
uint8_t max_speed_right; // Maximum speed variable right
uint8_t acl_left;
uint8_t acl_right;
uint8_t decl_left;
uint8_t decl_right;
/*****************************************************************************/
/**
* Just a small helper function to set speed params.
*/
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);
}
/**
* RC5 Data reception handler - this function is called automatically from the
* RP6lib if new RC5 Data has been received.
*/
void receiveRC5Data(RC5data_t rc5data)
#define Remote_Controll
#ifdef Remote_Controll
#define RC5_KEY_LEFT 4
#define RC5_KEY_RIGHT 6
#define RC5_KEY_FORWARDS 2
#define RC5_KEY_BACKWARDS 8
#define RC5_KEY_STOP 5
#define RC5_KEY_CURVE_LEFT 1
#define RC5_KEY_CURVE_RIGHT 3
#define RC5_KEY_CURVE_BACK_LEFT 7
#define RC5_KEY_CURVE_BACK_RIGHT 9
#define RC5_KEY_LEFT_MOTOR_FWD 32 //Ch+
#define RC5_KEY_LEFT_MOTOR_BWD 33 //Ch-
#define RC5_KEY_RIGHT_MOTOR_FWD 16 //Vol+
#define RC5_KEY_RIGHT_MOTOR_BWD 17 //Vol-
#define RC5_KEY_ALERT 0 //Auf der '0'-Taste blinken LEDs
#define RC5_KEY_LIGHT 13 //Auf der 'Stumm' gehen Postitions-LEDs an
#define RC5_KEY_LIGHT_OFF 34 //Auf der Pfeil-Taste gehen Lichter aus
#endif
{
// Output the received data:
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');
uint8_t movement_command = false; // used to store if we have received
// a movement command.
// Any other key is ignored!
//Für den Slave: ------------------------------------
lastRC5Reception.toggle_bit = rc5data.toggle_bit;
lastRC5Reception.device = rc5data.device;
lastRC5Reception.key_code = rc5data.key_code;
interrupt_status.RC5reception = true;
signalInterrupt();
//---------------------------------------------------
// Check which key is pressed:
switch(rc5data.key_code)
{
//hier sind halt alle meine Befehle drin...
//musste ich kürzen, da sonst der Text zu lang wurde (>20.000 Zeichen)
}
if(movement_command) // Did we receive a move command?
{
// Accelerate if neccessary:
if(getDesSpeedLeft() < max_speed_left) // If we have not reached the left maximum speed...
{ // ... accelerate!
moveAtSpeed(getDesSpeedLeft()+acl_left, getDesSpeedRight());
if(getDesSpeedLeft() < 10)
moveAtSpeed(10, getDesSpeedRight());
}
if(getDesSpeedRight() < max_speed_right) // If we have not reached the right maximum speed...
{
// ... accelerate!
moveAtSpeed(getDesSpeedLeft(), getDesSpeedRight()+acl_right);
if(getDesSpeedRight() < 10)
moveAtSpeed(getDesSpeedLeft(), 10);
}
// Start Stopwatch 1 - it starts decceleration after 250ms of no RC5 reception! (s. below)
setStopwatch1(0);
startStopwatch1();
}
}
/*****************************************************************************/
/**
* This function is called frequently out of the main loop and checks if
* Stopwatch1 has counted at least 250ms. If this is the case, decceleration is started
* and the Stopwatch is resetted and the progtam waits for next 250ms to pass by.
* Stopwatch1 ist set to 0 and started from the RC5 reception handler after
* each reception of a valid keycode. (s. above)
*/
void deccelerate(void)
{
if(getStopwatch1() > 250) // After 250ms with no reception...
{
if(getDesSpeedLeft() <= 10) // If left speed is less or equal than 10...
moveAtSpeed(0, getDesSpeedRight()); // ... stop the left motor
else // Otherwise continue to deccelerate:
moveAtSpeed(getDesSpeedLeft()-decl_left, getDesSpeedRight());
if(getDesSpeedRight() <= 10) // If right speed is less or equal than 10...
moveAtSpeed(getDesSpeedLeft(), 0); // ... stop the right motor
else // Otherwise continue to deccelerate:
moveAtSpeed(getDesSpeedLeft(), getDesSpeedRight()-decl_right);
if (getDesSpeedRight() == 0 && getDesSpeedLeft() == 0)
stopStopwatch1(); // Decceleration has finished!
max_speed_left = getDesSpeedLeft(); // Update max_speed value
max_speed_right = getDesSpeedRight(); // Update max_speed value
setLEDs(0b000000); // and clear LEDs
setStopwatch1(0);
}
// Make sure we don't move after Direction has changed and key is released too fast.
// This prevents the RP6 from moving when the direction has just changed and temporary saved
// speed value is written back again in the task_motionControl function.
if(getDesSpeedLeft() > max_speed_left)
{
if(getDesSpeedLeft() <= 10) // If left speed is less or equal than 10...
moveAtSpeed(0, getDesSpeedRight()); // ... stop the left motor
else // decelerate:
moveAtSpeed(getDesSpeedLeft()-decl_left, getDesSpeedRight());
}
if(getDesSpeedRight() > max_speed_right)
{
if(getDesSpeedRight() <= 10) // If right speed is less or equal than 10...
moveAtSpeed(getDesSpeedLeft(), 0); // ... stop the right motor
else // decelerate:
moveAtSpeed(getDesSpeedLeft(), getDesSpeedRight()-decl_right);
}
}
/*****************************************************************************/
// Main - The program starts here:
int main(void)
{
initRobotBase();
setLEDs(0b111111);
writeChar('\n');
writeString_P("RP6 controlled by RC5 TV Remote\n");
writeString_P("___________________________\n");
mSleep(500);
setLEDs(0b000000);
// Set the RC5 Receive Handler:
IRCOMM_setRC5DataReadyHandler(receiveRC5Data);
//Für den Slave:-----------------------------------
I2CTWI_initSlave(RP6BASE_I2C_SLAVE_ADR);
ACS_setStateChangedHandler(acsStateChanged);
BUMPERS_setStateChangedHandler(bumpersStateChanged);
IRCOMM_setRC5DataReadyHandler(receiveRC5Data);
//-------------------------------------------------
powerON();
//Output small usage instructions and the RC5 Codes:
writeString_P("\nYou can control your RP6 with the following RC5 Keycodes:");
writeString_P("\n-----------------------");
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-----------------------\n");
writeString_P("To change the key mapping, read the comments in the program source code!\n");
writeString_P("_________\nPlease make sure that your IR Remote Control really transmits RC5 code!\n");
startStopwatch2();
//Für den Slave:------------------------------
startStopwatch1();
disableACS();
setACSPwrOff();
status.byte = 0;
interrupt_status.byte = 0;
drive_status.byte = 0;
status.watchDogTimer = false;
status.wdtRequestEnable = false;
startStopwatch3();
startStopwatch4();
//-------------------------------------------
// Main loop
while(true)
{
task_update(); //Slave!
task_updateRegisters(); //Slave!
deccelerate(); // Call the deceleration function.
task_RP6System(); // Motion Control tasks etc.
}
return 0;
}
Wo liegt denn der Fehler?
Danke,
Fabian
Zitieren
Lesezeichen