Hier mal mein bisheriger Stand des Programms:
Leider reagiert der RP6 aber auf gar nix mehr...
Wo liegt denn der Fehler?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; }
Danke,
Fabian







Zitieren

Lesezeichen