Haben wir alles schon gemacht.
Nur die Stop Funktion macht Probleme, von daher wird es wohl an dem waitForTransmit liegen...
Ist erstmal egal, es gibt ein noch viel größeres Problem: Wenn man in der Hauptschleife die Infrarotsensoren abfragt und moveAtSpeed benutzt, funktionieren die Infrarotsensoren nicht mehr. Das hat garantiert nichts damit zu tun, dass die Motoren an sind.
Vielleicht wäre es doch einfacher, komplett auf die Fleury Library umzusteigen...
EDIT:
So, mit der Library von Peter Fleury auf der M32 Platine und dem Slave Programm aus den RP6 Examples auf der Base funktioniert alles. Wir werden, sobald wir alles, was mit der neuen Library nicht benötigt wird, aus dem Programm entfernt haben, hier alles posten.
EDIT2:
So:
1) Im Makefile Zeile 74 (dort, wo die Standard I²C Library eingebunden wird) entfernen oder auskommentieren
2) Die I²C Master Library von Peter Fleury herunterladen
3) Library (nur die Dateien twimaster.c und i2cmaster.h) in den Ordner einfügen, in den auch das Hauptprogramm und das Makefile ist
4) In twimaster.c I²C Geschwindigkeit und ggf. CPU Takt (16MHz, also 16000000Hz) festlegen
5) Ins Hauptprogramm zu den anderen includes schreiben:
Code:
#include "i2cmaster.h"
6) Ins Hauptprogramm die Zeilen
Code:
I2CTWI_initMaster(100);
I2CTWI_setRequestedDataReadyHandler(I2C_requestedDataReady);
I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);
durch das:
ersetzen.
7) Ggf. RP6Control_I2CMasterLib.c durch das ersetzen:
(ACHTUNG: move, rotate und setLEDs kann man dann nicht mehr benutzen, nur noch moveAtSpeed, changeDirection und stop. Natürlich kann man das alles ergänzen, wenn man will, wir benötigen das aber nicht, deshalb haben wir uns keine Arbeit damit gemacht)
Code:
// Includes:
#include "RP6Control_I2CMasterLib.h"
/*****************************************************************************/
// Sensors/ADCs:
// ADCs:
uint16_t adcBat;
uint16_t adcMotorCurrentLeft;
uint16_t adcMotorCurrentRight;
uint16_t adcLSL;
uint16_t adcLSR;
uint16_t adc0;
uint16_t adc1;
//Accelerometer:
int16_t threedbs_x;
int16_t threedbs_y;
int16_t threedbs_z;
//IR (MLX90614)
int16_t mlx90614_l;
int16_t mlx90614_r;
// Measured Speed:
uint8_t mleft_speed;
uint8_t mright_speed;
// Distance
uint16_t mleft_dist;
uint16_t mright_dist;
// Desired Speed:
uint8_t mleft_des_speed;
uint8_t mright_des_speed;
// Power
uint8_t mleft_power;
uint8_t mright_power;
uint8_t sensorBuf[24];
/**
* In order to use the same register names as in the RP6Lib, this
* function reads all ADC channels and all motor parameters into
* the same values as in the RP6Lib.
* Of course this function needs some time to read all these
* 24 registers via the I2C Bus.
*/
void getAllSensors(void)
{
i2c_start(I2C_RP6_BASE_ADR+0);
i2c_write(I2C_REG_POWER_LEFT);
i2c_rep_start(I2C_RP6_BASE_ADR+1);
for(uint8_t i = 0; i<23; i++)
{
sensorBuf[i] = i2c_readAck();
}
sensorBuf[23] = i2c_readNak();
i2c_stop();
mleft_power = sensorBuf[0];
mright_power = sensorBuf[1];
mleft_speed = sensorBuf[2];
mright_speed = sensorBuf[3];
mleft_des_speed = sensorBuf[4];
mright_des_speed = sensorBuf[5];
mleft_dist = sensorBuf[6] + (sensorBuf[7]<<8);
mright_dist = sensorBuf[8] + (sensorBuf[9]<<8);
adcLSL = sensorBuf[10] + (sensorBuf[11]<<8);
adcLSR = sensorBuf[12] + (sensorBuf[13]<<8);
adcMotorCurrentLeft = sensorBuf[14] + (sensorBuf[15]<<8);
adcMotorCurrentRight = sensorBuf[16] + (sensorBuf[17]<<8);
adcBat = sensorBuf[18] + (sensorBuf[19]<<8);
adc0 = sensorBuf[20] + (sensorBuf[21]<<8);
adc1 = sensorBuf[22] + (sensorBuf[23]<<8);
}
/**
*
*/
void getLightSensors(void)
{
i2c_start_wait(I2C_RP6_BASE_ADR);
i2c_write(I2C_REG_ADC_LSL_L);
i2c_rep_start(I2C_RP6_BASE_ADR+1);
for(uint8_t i = 0; i<3; i++)
{
sensorBuf[i] = i2c_readAck();
}
sensorBuf[3] = i2c_readNak();
i2c_stop();
adcLSL = sensorBuf[0] + (sensorBuf[1]<<8);
adcLSR = sensorBuf[2] + (sensorBuf[3]<<8);
}
/**
*
*/
void getADCs(void)
{
i2c_start_wait(I2C_RP6_BASE_ADR);
i2c_write(I2C_REG_ADC_ADC0_L);
i2c_rep_start(I2C_RP6_BASE_ADR+1);
for(uint8_t i = 0; i<3; i++)
{
sensorBuf[i] = i2c_readAck();
}
sensorBuf[3] = i2c_readNak();
i2c_stop();
adc0 = sensorBuf[0] + (sensorBuf[1]<<8);
adc1 = sensorBuf[2] + (sensorBuf[3]<<8);
}
/**
*
*/
void get3DBS(void)
{
i2c_start(I2C_3DBS_ADR);
i2c_write(I2C_REG_3DBS);
i2c_rep_start(I2C_3DBS_ADR+1);
for(uint8_t i = 0; i<5; i++)
{
sensorBuf[i] = i2c_readAck();
}
sensorBuf[5] = i2c_readNak();
i2c_stop();
threedbs_x = sensorBuf[1];//i2c_readAck() + (i2c_readAck()<<8);
threedbs_y = sensorBuf[3];//i2c_readAck() + (i2c_readAck()<<8);
threedbs_z = sensorBuf[5];//i2c_readAck() + (i2c_readNak()<<8);
}
/*
*
*/
void getIR(void)
{
i2c_start_wait(I2C_MLX90614_L);
i2c_write(I2C_REG_MLX90614);
i2c_rep_start((I2C_MLX90614_L)+1);
for(uint8_t i = 0; i<2; i++)
{
sensorBuf[i] = i2c_readAck();
}
sensorBuf[2] = i2c_readNak();
i2c_stop();
// This masks off the error bit of the high byte, then moves it left 8 bits and adds the low byte.
int16_t tempdata = (((sensorBuf[1] & 0x007F) << 8) + sensorBuf[0]);
tempdata = (tempdata * 2)-1;
mlx90614_l = tempdata - 27315;
i2c_start_wait(I2C_MLX90614_R);
i2c_write(I2C_REG_MLX90614);
i2c_rep_start((I2C_MLX90614_R)+1);
for(uint8_t i = 0; i<2; i++)
{
sensorBuf[i] = i2c_readAck();
}
sensorBuf[2] = i2c_readNak();
i2c_stop();
// This masks off the error bit of the high byte, then moves it left 8 bits and adds the low byte.
tempdata = (((sensorBuf[1] & 0x007F) << 8) + sensorBuf[0]);
tempdata = (tempdata * 2)-1;
mlx90614_r = tempdata - 27315;
}
/*
*
*/
void getDists(void)
{
i2c_start_wait(I2C_RP6_BASE_ADR);
i2c_write(I2C_REG_DIST_LEFT_L);
i2c_rep_start(I2C_RP6_BASE_ADR+1);
for(uint8_t i = 0; i<3; i++)
{
sensorBuf[i] = i2c_readAck();
}
sensorBuf[3] = i2c_readNak();
i2c_stop();
mleft_dist = sensorBuf[0] + (sensorBuf[1]<<8);
mright_dist = sensorBuf[2] + (sensorBuf[3]<<8);
}
/*****************************************************************************/
/*****************************************************************************/
// Movement functions:
/**
* Move at speed function - just the same as with RP6Lib!
*/
void moveAtSpeed(uint8_t desired_speed_left, uint8_t desired_speed_right)
{
i2c_start_wait(I2C_RP6_BASE_ADR);
i2c_write(0);
i2c_write(CMD_MOVE_AT_SPEED);
i2c_write(desired_speed_left);
i2c_write(desired_speed_right);
i2c_stop();
}
/**
* Change direction function - just the same as with RP6Lib!
*/
void changeDirection(uint8_t dir)
{
i2c_start_wait(I2C_RP6_BASE_ADR);
i2c_write(0);
i2c_write(CMD_CHANGE_DIR);
i2c_write(dir);
i2c_stop();
}
/**
* Stop function - just the same as with RP6Lib!
*/
void stop(void)
{
i2c_start_wait(I2C_RP6_BASE_ADR);
i2c_write(0);
i2c_write(CMD_STOP);
i2c_stop();
}
und RP6Control_I2CMasterLib.h durch das ersetzen:
Code:
#ifndef RP6CONTROL_I2CMASTERLIB_H
#define RP6CONTROL_I2CMASTERLIB_H
/*****************************************************************************/
// Includes:
#include "RP6ControlLib.h"
// define the RP6 Base address here:
#define I2C_RP6_BASE_ADR 10
//other I2C adresses:
#define I2C_3DBS_ADR 0x70
#define I2C_MLX90614_L 0x55<<1
#define I2C_MLX90614_R 0x5A<<1
/*****************************************************************************/
// 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_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
//Commands od other I2C devices:
#define I2C_REG_3DBS 0x02
#define I2C_REG_MLX90614 0x07
#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 getMotorRight() (OCR1A)
#define getMotorLeft() (OCR1B)
//Encoders:
extern uint8_t mleft_speed;
extern uint8_t mright_speed;
// Distance
extern uint16_t mleft_dist;
extern uint16_t mright_dist;
// Desired Speed:
extern uint8_t mleft_des_speed;
extern uint8_t mright_des_speed;
// Power
extern uint8_t mleft_power;
extern uint8_t mright_power;
#define getLeftSpeed() (mleft_speed)
#define getRightSpeed() (mright_speed)
#define getLeftDistance() (mleft_dist)
#define getRightDistance() (mright_dist)
#define getDesSpeedLeft() (mleft_des_speed)
#define getDesSpeedRight() (mright_des_speed)
//Direction:
#define FWD 0
#define BWD 1
#define LEFT 2
#define RIGHT 3
#define getDirection() (drive_status.direction)
#define INT0_STATUS_CHECK 0
/*****************************************************************************/
//
void initI2C_RP6Lib(void);
uint8_t checkRP6Status(uint8_t dataRequestID);
void I2C_transmissionError(uint8_t errorState);
void task_checkINT0(void);
#define NON_BLOCKING 0
#define BLOCKING 1
void rotate(uint8_t desired_speed, uint8_t dir, uint16_t angle, uint8_t blocking);
void move(uint8_t desired_speed, uint8_t dir, uint16_t distance, uint8_t blocking);
void moveAtSpeed(uint8_t desired_speed_left, uint8_t desired_speed_right);
void changeDirection(uint8_t dir);
void stop(void);
void getADCs(void);
void getDists(void);
void get3DBS(void);
void getIR(void);
#define isMovementComplete() (drive_status.movementComplete)
extern uint8_t bumper_left;
extern uint8_t bumper_right;
extern uint8_t obstacle_left;
extern uint8_t obstacle_right;
#define isObstacleLeft() (obstacle_left)
#define isObstacleRight() (obstacle_right)
extern uint16_t adcBat;
extern uint16_t adcMotorCurrentLeft;
extern uint16_t adcMotorCurrentRight;
extern uint16_t adcLSL;
extern uint16_t adcLSR;
extern uint16_t adc0;
extern uint16_t adc1;
//Accelerometer:
extern int16_t threedbs_x;
extern int16_t threedbs_y;
extern int16_t threedbs_z;
//IR (MLX90614)
extern int16_t mlx90614_l;
extern int16_t mlx90614_r;
#define TOGGLEBIT 32
typedef union {
uint16_t data;
struct {
uint8_t key_code:6;
uint8_t device:5;
uint8_t toggle_bit:1;
uint8_t reserved:3;
};
} RC5data_t;
void IRCOMM_sendRC5(uint8_t adr, uint8_t data);
void IRCOMM_setRC5DataReadyHandler(void (*rc5Handler)(RC5data_t));
void ACS_setStateChangedHandler(void (*acsHandler)(void));
void MOTIONCONTROL_setStateChangedHandler(void (*motionControlHandler)(void));
void BUMPERS_setStateChangedHandler(void (*bumperHandler)(void));
void WDT_setRequestHandler(void (*requestHandler)(void));
void getAllSensors(void);
void getLightSensors(void);
#endif
nun sollte alles funktionieren. Bei Fragen einfach fragen! 
Zitieren
Lesezeichen