Hallo!
Ich habe jetzt versucht, das Programm zur RC5 Steuerung umzuschreiben.
Hier mal das ganze Programm:
Code:
/*****************************************************************************/
// Includes:
#include "RP6ControlLib.h" // The RP6 Control Library.
// Always needs to be included!
#include "RP6I2CmasterTWI.h" // I2C Master Library
#include "RP6Control_I2CMasterLib RC.h"
/*****************************************************************************/
#define I2C_RP6_BASE_ADR 10 // The default address of the Slave Controller
// on the RP6 Mainboard
/*****************************************************************************/
/*****************************************************************************/
// If you only want to look at the RC5 Keycodes received from the Remote
// Control, uncomment this line:
#define DO_NOT_MOVE
// The RP6 will not move then!
/*****************************************************************************/
// These are the same command definitions as you can find them in the
// I2C Bus Slave Example program for RP6Base:
#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
#define CMD_POWER_OFF 0
#define CMD_POWER_ON 1
#define CMD_CONFIG 2
#define CMD_SETLEDS 3
#define CMD_STOP 4
#define CMD_MOVE_AT_SPEED 5
#define CMD_CHANGE_DIR 6
#define CMD_MOVE 7
#define CMD_ROTATE 8
#define CMD_SET_ACS_POWER 9
#define CMD_SEND_RC5 10
#define CMD_SET_WDT 11
#define CMD_SET_WDT_RQ 12
#define ACS_PWR_OFF 0
#define ACS_PWR_LOW 1
#define ACS_PWR_MED 2
#define ACS_PWR_HIGH 3
/***************************************************************************/
//Einige Definitionen:
#define RC_RFT
#ifdef RC_RFT // RC Type: RFT Videorecorder Fernbedienung
#define RC5_KEY_LEFT 4
#define RC5_KEY_RIGHT 6
#define RC5_KEY_FORWARDS 2
#define RC5_KEY_BACKWARDS 8
#define RC5_KEY_STOP 12
#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 10
#define RC5_KEY_LEFT_MOTOR_BWD 29
#define RC5_KEY_RIGHT_MOTOR_FWD 62
#define RC5_KEY_RIGHT_MOTOR_BWD 13
#define RC5_KEY_Lighton 32
#define RC5_KEY_Lightoff 33
#endif
/*****************************************************************************/
uint8_t light = 0;
// 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);
}
/*****************************************************************************/
// Einige uint8_t:
uint8_t rc5;
uint8_t dir = 0;
uint8_t runLEDs = 1;
/*****************************************************************************/
void receiveRC5Data(RC5data_t rc5data)
{
// Output the received data:
writeString_P("RC5 Reception Toggle Bit:");
writeChar(rc5data.toggle_bit + '0');
writeString_P(" | Device Address:");
writeInteger(rc5data.device, DEC);
writeString_P(" | Key Code:");
writeInteger(rc5data.key_code, DEC);
rc5 = rc5data.key_code;
writeChar('\n');
#ifndef DO_NOT_MOVE // used to disable movement if you want to
// look at the received RC5 codes only (s. above).
uint8_t movement_command = false; // used to store if we have received
// a movement command.
// Any other key is ignored!
// Check which key is pressed:
switch(rc5data.key_code)
{
case RC5_KEY_Lightoff: // Licht aus:
writeString_P("Licht Aus\n");
light = 0;
setRP6LEDs(0b011000);
break;
case RC5_KEY_Lighton: // Licht an:
writeString_P("Licht\n");
light = 1;
setRP6LEDs(0b111111);
break;
case RC5_KEY_LEFT: // Turn left:
writeString_P("LEFT\n");
setDefaultSpeedParameters();
max_speed_left = MAX_SPEED_TURN;
max_speed_right = max_speed_left;
changeDirection(LEFT);
setRP6LEDs(0b100000);
movement_command = true; // Store that we have received a movement command!
break;
case RC5_KEY_RIGHT: // Turn right:
writeString_P("RIGHT\n");
setDefaultSpeedParameters();
max_speed_left = MAX_SPEED_TURN;
max_speed_right = max_speed_left;
changeDirection(RIGHT);
setRP6LEDs(0b000100);
movement_command = true;
break;
case RC5_KEY_FORWARDS: // Move forwards
writeString_P("FORWARDS\n");
setDefaultSpeedParameters();
changeDirection(FWD);
setRP6LEDs(0b100100);
movement_command = true;
break;
case RC5_KEY_BACKWARDS: // Move backwards
writeString_P("BACKWARDS\n");
setDefaultSpeedParameters();
changeDirection(BWD);
setRP6LEDs(0b001001);
movement_command = true;
break;
case RC5_KEY_STOP: // Stop!
writeString_P("STOP\n");
max_speed_left = 0;
max_speed_right = max_speed_left;
moveAtSpeed(0,0);
setRP6LEDs(0b011011);
movement_command = true;
break;
case RC5_KEY_CURVE_LEFT: // Drive curve left - forwards
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);
setRP6LEDs(0b110100);
movement_command = true;
break;
case RC5_KEY_CURVE_RIGHT: // Drive curve right - forwards
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);
setRP6LEDs(0b100110);
movement_command = true;
break;
case RC5_KEY_CURVE_BACK_LEFT: // Drive curve left - backwards
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);
setRP6LEDs(0b011001);
movement_command = true;
break;
case RC5_KEY_CURVE_BACK_RIGHT: // Drive curve right - backwards
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);
setRP6LEDs(0b001011);
movement_command = true;
break;
case RC5_KEY_LEFT_MOTOR_FWD: // Only left motor on - forwards
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);
setRP6LEDs(0b110000);
movement_command = true;
break;
case RC5_KEY_LEFT_MOTOR_BWD: // Only left motor on - backwards
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);
setRP6LEDs(0b101000);
movement_command = true;
break;
case RC5_KEY_RIGHT_MOTOR_FWD: // Only right motor on - forwards
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);
setRP6LEDs(0b000110);
movement_command = true;
break;
case RC5_KEY_RIGHT_MOTOR_BWD: // Only right motor on - backwards
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);
setRP6LEDs(0b000101);
movement_command = true;
break;
}
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 2 - it starts decceleration after 250ms of no RC5 reception! (s. below)
setStopwatch2(0);
startStopwatch2();
}
#endif
}
/********************************************************************************/
void deccelerate(void)
{
if(getStopwatch2() > 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)
stopStopwatch2(); // Decceleration has finished!
max_speed_left = getDesSpeedLeft(); // Update max_speed value
max_speed_right = getDesSpeedRight(); // Update max_speed value
setRP6LEDs(0b000000); // and clear LEDs
setStopwatch2(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);
}
}
/********************************************************************************/
void I2C_requestedDataReady(uint8_t dataRequestID)
{
// We need to check if this is an INT0 Request from the Robot Base unit.
// The Function call inside the if condition returns true if it was an
// interrupt request, so we have to negate it here and if it was NOT
// an interrupt request from the Robot base we can check any other sensors
// from which you may have requested data...
if(!checkRP6Status(dataRequestID))
{
// Here you can Check other sensors/microcontrollers with their own
// request IDs - if there are any...
}
}
/*****************************************************************************/
void task_LCDHeartbeat(void)
{
static uint8_t heartbeat = false;
if(getStopwatch1() > 500)
{
if(heartbeat)
{
clearPosLCD(0, 15, 1);
heartbeat = false;
}
else
{
setCursorPosLCD(0, 15);
writeStringLCD_P("*");
heartbeat = true;
}
setStopwatch1(0);
}
}
/*****************************************************************************/
// 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).
* The most common mistakes are:
* - using the wrong address for the slave
* - slave not active or not connected to the I2C-Bus
* - too fast requests for a slower slave
* Be sure to check this if you get I2C errors!
*/
void I2C_transmissionError(uint8_t errorState)
{
writeString_P("\nI2C ERROR - TWI STATE: 0x");
writeInteger(errorState, HEX);
writeChar('\n');
}
/****************************************************************************/
void task_lauflicht(void)
{
if(getStopwatch4() > 100)
{
setLEDs(runLEDs);
if(dir == 0)
runLEDs <<= 1;
else
runLEDs >>= 1;
if(runLEDs > 7 )
dir = 1;
else if (runLEDs < 2 )
dir = 0;
setStopwatch4(0);
}
}
/*****************************************************************************/
/*****************************************************************************/
// Main function - The program starts here:
int main(void)
{
initRP6Control(); // Always call this first! The Processor will not work
// correctly otherwise.
initLCD();
writeString_P("\n\nRP6 CONTROL M32 I2C Master Beispiel Programm!\n");
writeString_P("\nInterrupts und RC5 Steuerung\n");
// IMPORTANT:
I2CTWI_initMaster(100); // Initialize the TWI Module for Master operation
// with 100kHz SCL Frequency
// Also very important in this example:
// Register the event handlers:
I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);
IRCOMM_setRC5DataReadyHandler(receiveRC5Data);
I2CTWI_setRequestedDataReadyHandler(I2C_requestedDataReady);
sound(180,80,25);
sound(220,80,25);
setLEDs(0b1111);
showScreenLCD("################", "################");
mSleep(500);
showScreenLCD("I2C-Master", "Example Program 2");
mSleep(1000);
setLEDs(0b0000);
showScreenLCD("ACS Status:", "");
// ---------------------------------------
// Set ACS to medium power:
I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_ACS_POWER, ACS_PWR_MED);
// Enable Software Watchdog timer on Slave:
I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_WDT, true);
// (This timer stops all operations of the Slave if the Master Controller
// does not react on interrupt requests after a few seconds. This can
// prevent damage if the Master controller locks up but the Slave has still
// the order to drive at high speed... it would maybe crash against a wall
// at high speeds or worser things... )
// ---------------------------------------
startStopwatch1(); // For LCDHeartbeat function
startStopwatch4(); // Für Lauflicht
while(true)
{
// You need to call task_I2CTWI frequently out of the
// main loop - otherwise the I2C Bus request functions don't work.
task_checkINT0();
deccelerate(); // Call the deceleration function.
//test();
task_I2CTWI();
task_LCDHeartbeat();
task_lauflicht();
}
return 0;
}
Ich habe das schritt für schritt aufgebaut, und jeweils immer probiert. Das Resultat ist, dass er schon auf die verschiedenen Tasten reagiert, allerdings das fahren klappt überhaupt nicht. Er steuert immer nur kurz den den rechten Motor an (Speed ungefähr 10), aber das beschleunigen klappt überhaupt nicht. Ich hoffe, ihr könnt mir auch hierbei helfen.
Meine Vermutung ist ja, dass die Übertragung des I2C zu lange dauert und der Motor sozusagen auf 10 springt, abfällt, auf 10 springt, usw. Kann man das vielleicht beheben.
Vielen Dank
MfG RP6fahrer
Berechtigungen
Zitieren
Lesezeichen