Code:
// includes:
#include "RP6RobotBaseLib.h"
#include "RP6I2CmasterTWI.h"
// definitions:
#define CMPS03 0xC0
#define SRF02 0xE0
#define DS1621_Write 0x9E
#define DS1621_Read 0x9F
#define I2C_RP6_Control_ADR 0x0A
#define attachdistance 8
// movements
#define STOP 0
#define FORWARD 1
#define LEFT 2
#define RIGHT 3
#define HARDLEFT 4
#define HARDRIGHT 5
#define TURNLEFT 6
#define TURNRIGHT 7
#define TURN 8
#define UTURN 9
#define BACKWARD 10
#define OVERRIDE 11
// cruise states
#define READY 0
#define CRUISING 1
#define CORRECTING 2
// avoid states
#define FREE 0
#define AVOID 1
#define ROTATE 2
// escape states
#define FREE 0
#define OBSTACLE 1
#define BACKWARD 10
#define ROTATE 2
// speeds
#define CRUISE 100
#define FAST 90
#define MEDIUM 60
#define SLOW 30
// global variables:
uint16_t area[36] = {0}; //memory for the surrounding area
uint8_t avoid_state = FREE;
uint8_t cruise_state = READY;
uint16_t desired_direction = 1800; //general target direction
uint16_t global_direction = 1800;
uint8_t escape_state = FREE;
uint8_t obstacle_count = 0; //object counter
uint8_t movement = STOP; //current movement to do
uint8_t move_enable = false; //is he allowed to move
// funktion deklarations:
void acsStateChanged(void);
void adjustment(void);
void avoid(void);
void cruise(void);
uint16_t distance_SRF02(void);
uint16_t distance_SRF02_blocking(void);
uint16_t direction_calculator(void);
uint16_t direction_CMPS03(void);
void escape(void);
void frontlight(void);
void I2C_dataupload(void);
void movecontroller(void);
void task_expansion(void);
uint16_t temperature_DS1621(void);
uint8_t turndirection(void);
// funktions:
void I2C_dataupload(void)
{
if(!isStopwatch4Running())
{
setStopwatch4(0);
startStopwatch4();
}
if(getStopwatch4()>1000)
{
static uint8_t uploadRegisters[17] = {0};
static uint16_t temperature;
static uint16_t direction = 0;
static uint16_t distance = 0;
temperature = temperature_DS1621();
if (obstacle_count<3)
{
direction = direction_CMPS03();
distance = distance_SRF02_blocking();
}
uploadRegisters[0] = 0; //start Register
uploadRegisters[1] = (uint8_t)(adcLSL); //Low-Byte (im Index des Empfängers ist alles um 1 versetzt)
uploadRegisters[2] = (uint8_t)(adcLSL>>8); //High-Byte
uploadRegisters[3] = (uint8_t)(adcLSR); //...
uploadRegisters[4] = (uint8_t)(adcLSR>>8);
uploadRegisters[5] = (uint8_t)(adcMotorCurrentLeft);
uploadRegisters[6] = (uint8_t)(adcMotorCurrentLeft>>8);
uploadRegisters[7] = (uint8_t)(adcMotorCurrentRight);
uploadRegisters[8] = (uint8_t)(adcMotorCurrentRight>>8);
uploadRegisters[9] = (uint8_t)(adcBat);
uploadRegisters[10] = (uint8_t)(adcBat>>8);
uploadRegisters[11] = (uint8_t)(temperature); // 0 or 0.5°C
uploadRegisters[12] = (uint8_t)(temperature>>8); // whole degrees in C
uploadRegisters[13] = (uint8_t)(distance);
uploadRegisters[14] = (uint8_t)(distance>>8);
uploadRegisters[15] = (uint8_t)(direction); // 10*whole degrees
uploadRegisters[16] = (uint8_t)(direction/256);
I2CTWI_transmitBytes(I2C_RP6_Control_ADR,uploadRegisters,17); //dataupload
I2CTWI_transmitByte(I2C_RP6_Control_ADR,0); //movedownload
move_enable = I2CTWI_readByte(I2C_RP6_Control_ADR);
setStopwatch4(0);
}
}
uint16_t temperature_DS1621(void) // gibt die 256 fache Umgebungstemperatur zurück
{
uint8_t cmpsbuffer[2] = {0};
I2CTWI_transmitByte(DS1621_Write, 0xEE); // Befehl geben die Temperatur bereit zu stellen
I2CTWI_transmitByte(DS1621_Write, 0xAA); // Ab dem 170. Register Daten anfordern
I2CTWI_readBytes(DS1621_Read,cmpsbuffer,2); // und in dem Puffer Array speichern
return (cmpsbuffer[0]-1) * 256 + cmpsbuffer[1]; //1° Temperaturanpassung
}
uint16_t distance_SRF02(void) //Distanzmessung mit Stopwatches
{
static uint8_t measureactive = false; //Messungsstatus
uint8_t srfbuffer[2]; //Speicher für die übertragenen Bytes
static uint16_t distance = 0;
if (measureactive == false)
{
setStopwatch6(0);
startStopwatch6();
I2CTWI_transmit2Bytes(SRF02, 0, 81); // SRF02 bekommt im Register 0 den Befehl in cm zu messen
measureactive = true;
}
if (getStopwatch6() >= 70 && measureactive == true)
{
I2CTWI_transmitByte(SRF02, 2); // Ab dem zweiten Reigster Daten anfordern
I2CTWI_readBytes(SRF02, srfbuffer, 2); // und in dem Array speichern
distance = srfbuffer[0] * 256 + srfbuffer[1]-attachdistance;
measureactive = false;
stopStopwatch6();
// hiermit werden Messfehler korregiert, da negative Werte sonst bei anderen funktionen zu Problemen führen
if (distance == -attachdistance)
distance = 0;
//Anzeige der Distanz auf den LEDs
statusLEDs.LED1=0;
statusLEDs.LED2=0;
statusLEDs.LED3=0;
statusLEDs.LED4=0;
statusLEDs.LED5=0;
statusLEDs.LED6=0;
if (((distance % 100)/25 >= 1) || (distance >= 400)) // cm werden auf der einen Seite dargestellt
statusLEDs.LED1=1;
if (((distance % 100)/25 >= 2) || (distance >= 400))
statusLEDs.LED2=1;
if (((distance % 100)/25 >= 3) || (distance >= 400))
statusLEDs.LED3=1; // m auf der anderen
if (distance / 100 >= 1)
statusLEDs.LED4=1;
if (distance / 100 >= 2)
statusLEDs.LED5=1;
if (distance / 100 >= 3)
statusLEDs.LED6=1;
updateStatusLEDs();
}
return distance;
}
uint16_t distance_SRF02_blocking(void) //Distanzmessung
{
uint8_t srfbuffer[2]; //Speicher für die übertragenen Bytes
I2CTWI_transmit2Bytes(SRF02, 0, 81); // SRF02 bekommt im Register 0 den Befehl in cm zu messen
mSleep(65);
I2CTWI_transmitByte(SRF02, 2); // Ab dem zweiten Reigster Daten anfordern
I2CTWI_readBytes(SRF02, srfbuffer, 2); // und in dem Array speichern
return srfbuffer[0] * 256 + srfbuffer[1]-attachdistance;
}
uint16_t direction_CMPS03(void) //Richtungsbestimmung
{
uint8_t cmpsbuffer[2];
I2CTWI_transmitByte(CMPS03, 2); // Ab dem zweiten Reigster Daten anfordern
I2CTWI_readBytes(CMPS03, cmpsbuffer, 2); // und in dem Array speichern
return ((cmpsbuffer[0] * 256) + cmpsbuffer[1]);
}
void allroundscan(void) // Umgebungsscan wird in area[] gespeichert
{
uint16_t index = direction_CMPS03() / 100; // den Index and der aktuellen Richtung starten lassen
uint16_t counter = 0;
uint16_t direction = 0;
uint16_t distance = 0;
writeString_P("\n\tAreascan initiated\n*******************************************\n");
stop();
changeDirection(RIGHT);
moveAtSpeed(40,40);
while(counter < 36)
{
task_RP6System();
task_expansion();
direction = direction_CMPS03() / 100;
// Drehrichtung bei übersprungenen Messungen korregieren (funktioniert nicht über den Nullsprung!!!) ausbaufähig
if ((direction > (index + 1)) && (index > 2))
changeDirection(LEFT);
if ((direction < (index -2)) && (index >= 2))
changeDirection(RIGHT);
// Messwert abspeichern
if (direction == index)
{
distance = distance_SRF02_blocking();
if (distance >= 1000 || obstacle_left || obstacle_right) //10m oder ein vom ACS entdecktes Objekt schließt diese Richtung aus.
area[index] = 0;
else
area[index] = distance;
writeString_P("Areascan Value [");
writeInteger(index*10,DEC);
writeString_P("]\t=");
writeInteger(area[index],DEC);
writeString_P("\n");
changeDirection(RIGHT);
index++;
counter++;
}
// Nullsprung
if (index >= 36)
index = 0;
}
stop();
writeString_P("\tAreascan completed\n*******************************************\n");
}
uint16_t direction_calculator(void) // berechnet die beste neue Richtung aus area[]
{
uint16_t maximumsum = 0;
uint16_t newdirection = 0;
uint8_t i = 0;
uint8_t count;
if (desired_direction > 1800)
count = (desired_direction - 1800) / 100;
else
count = (1800 - desired_direction) / 100;
uint8_t relevance[36] = {0};
for(i=0;i<36;i++) //relevance calculation
{
if(i<=18)
relevance[count] = i+1;
if(i>18)
relevance[count] = 36-i+1;
count++;
if (count>=36)
count=0;
}
for(i=0;i<36;i++) //measurand smoothing
{
if(i==0)
{
if(1.5*(area[35]+area[1])<area[0])
{
writeString_P("Measurand smoothed: area["); writeInteger(i,DEC);writeString_P("]=");writeInteger(area[i],DEC);
area[i] = (area[35]+area[1])/2;
writeString_P("-->"); writeInteger(area[i],DEC);writeString_P("\n");
}
}
else
{
if(1.5*(area[i-1]+area[i+1])<area[i])
{
writeString_P("Measurand smoothed: area["); writeInteger(i,DEC);writeString_P("]=");writeInteger(area[i],DEC);
area[i] = (area[i-1]+area[i+1])/2;
writeString_P("-->"); writeInteger(area[i],DEC);writeString_P("\n");
}
}
}
for(i=0;i<36;i++) //final direction calculation
{
if(i==0)
{
if(area[35]*relevance[35]/10 + area[i]*relevance[i]/10 + area[i+1]*relevance[i+1]/10 > maximumsum)
{
maximumsum = area[35]*relevance[35]/10 + area[i]*relevance[i]/10 + area[i+1]*relevance[i+1]/10;
newdirection = i;
}
}
else
{
if(area[i-1]*relevance[i-1]/10 + area[i]*relevance[i]/10 + area[i+1]*relevance[i+1]/10 > maximumsum)
{
maximumsum = area[i-1]*relevance[i-1]/10 + area[i]*relevance[i]/10 + area[i+1]*relevance[i+1]/10;
newdirection = i;
}
}
writeString_P("\nMaximum Sum ="); writeInteger(maximumsum,DEC); writeString_P("\t New Direction ="); writeInteger(newdirection*10,DEC);
}
writeString_P("\n\tDirection Calculation completed \n*******************************************\n");
return newdirection*100;
}
void cruise(void)
{
if (avoid_state == FREE && escape_state == FREE)
{
if (cruise_state == READY)
{
cruise_state = CRUISING;
movement = FORWARD;
setStopwatch5(0);
startStopwatch5();
statusLEDs.LED1 = true;
statusLEDs.LED4 = true;
updateStatusLEDs();
}
uint16_t direction = direction_CMPS03();
if ((cruise_state == CRUISING || cruise_state == CORRECTING) && getStopwatch5() >= 1800)
{
if (direction > desired_direction + 30)
{
cruise_state = CORRECTING;
statusLEDs.LED1 = false;
statusLEDs.LED4 = false;
updateStatusLEDs();
if (direction - desired_direction > 3450 && direction - desired_direction <= 3600 && (movement != HARDRIGHT || movement != TURNRIGHT))
movement = RIGHT;
if (direction - desired_direction > 2700 && direction - desired_direction <= 3450 && (movement != TURNRIGHT))
movement = HARDRIGHT;
if (direction - desired_direction > 1800 && direction - desired_direction <= 2700)
movement = TURNRIGHT;
if (direction - desired_direction > 900 && direction - desired_direction <= 1800)
movement = TURNLEFT;
if (direction - desired_direction > 150 && direction - desired_direction <= 900 && (movement != TURNLEFT))
movement = HARDLEFT;
if (direction - desired_direction > 0 && direction - desired_direction <= 150 && (movement != HARDLEFT || movement != TURNLEFT))
movement = LEFT;
}
if (desired_direction > direction + 30)
{
cruise_state = CORRECTING;
statusLEDs.LED1 = false;
statusLEDs.LED4 = false;
updateStatusLEDs();
if (desired_direction - direction > 3450 && desired_direction - direction <= 3600 && (movement != HARDLEFT || movement != TURNLEFT))
movement = LEFT;
if (desired_direction - direction > 2700 && desired_direction - direction <= 3450 && movement != TURNLEFT)
movement = HARDLEFT;
if (desired_direction - direction > 1800 && desired_direction - direction <= 2700)
movement = TURNLEFT;
if (desired_direction - direction > 900 && desired_direction - direction <= 1800)
movement = TURNRIGHT;
if (desired_direction - direction > 150 && desired_direction - direction <= 900 && movement != TURNRIGHT)
movement = HARDRIGHT;
if (desired_direction - direction > 0 && desired_direction - direction <= 150 && (movement != HARDRIGHT || movement != TURNRIGHT))
movement = RIGHT;
}
}
if (cruise_state == CORRECTING)
{
if (direction - desired_direction >= -30 || direction - desired_direction <= 30) // abweichung kleiner 3°
cruise_state = READY;
}
}
}
void avoid(void)
{
if (escape_state == FREE)
{
if (obstacle_left && obstacle_right && avoid_state != ROTATE)
{
avoid_state = ROTATE;
movement = TURN;
writeString_P("\nObstacle Counter= ");
++obstacle_count;
writeInteger(obstacle_count,DEC);
writeString_P("\n");
}
else if (obstacle_left)
{
avoid_state = AVOID;
movement = HARDRIGHT;
}
else if (obstacle_right)
{
avoid_state = AVOID;
movement = HARDLEFT;
}
if (avoid_state == AVOID && (!obstacle_left && !obstacle_right))
{
avoid_state = FREE;
cruise_state = READY;
}
}
}
void escape(void)
{
if (escape_state == FREE && (bumper_left || bumper_right))
{
stop();
escape_state = OBSTACLE;
writeString_P("Obstacle Counter= ");
++obstacle_count;
writeInteger(obstacle_count,DEC);
writeString_P("\n");
}
if (escape_state == OBSTACLE)
{
if (bumper_left && bumper_right)
{
escape_state = BACKWARD;
movement = TURN;
}
else if (bumper_left)
{
escape_state = BACKWARD;
movement = TURNRIGHT;
}
else if (bumper_right)
{
escape_state = BACKWARD;
movement = TURNLEFT;
}
else
escape_state = FREE;
}
}
uint8_t turndirection(void) // bestimmt welche Drehrichtung eher in Sollrichtung liegt
{
uint16_t direction = direction_CMPS03();
if (direction >= desired_direction)
{
if (direction - desired_direction >= 1800)
return TURNRIGHT;
else
return TURNLEFT;
}
if (desired_direction > direction)
{
if (desired_direction - direction >= 1800)
return TURNLEFT;
else
return TURNRIGHT;
}
return STOP;
}
void adjustment(void) // turns the robot to the desired direction on the shortest way
{
stop();
uint16_t direction = direction_CMPS03();
while(direction / 100 != desired_direction / 100)
{
task_RP6System();
task_expansion();
direction = direction_CMPS03();
moveAtSpeed(MEDIUM,MEDIUM);
writeString_P("\nCurrent Direction\t");
writeInteger(direction,DEC);
writeString_P("\tDesired Direction\t");
writeInteger(desired_direction,DEC);
if (direction > desired_direction)
{
if (direction - desired_direction >= 1800)
changeDirection(RIGHT);
else
changeDirection(LEFT);
}
if (direction < desired_direction)
{
if (desired_direction - direction >= 1800)
changeDirection(LEFT);
else
changeDirection(RIGHT);
}
}
stop();
}
void movecontroller(void) // gets the movement whishes from other funktions and controlls the engines
{
if (obstacle_count >= 3) // Funktionen zur Richtungsfindung
{
stop();
movement = OVERRIDE;
allroundscan();
desired_direction = direction_calculator();
adjustment();
obstacle_count = 0;
movement = STOP;
}
if (movement != OVERRIDE) // Ausweichfunktionen
{
cruise();
avoid();
escape();
changeDirection(FWD);
}
if (movement == TURN) // Entscheidung welche Drehrichtung vorteilhafter ist
movement = turndirection();
// Ausführung der Gewünschten Fahrtbewegung
if (escape_state == BACKWARD)
{
move(MEDIUM, BWD, DIST_MM(100), BLOCKING);
escape_state = ROTATE;
}
if (movement == TURNRIGHT)
{
if (escape_state == ROTATE || avoid_state == ROTATE)
{
stop();
rotate(MEDIUM, RIGHT, 90, BLOCKING);
escape_state = FREE;
avoid_state = FREE;
cruise_state = READY;
}
else
{
changeDirection(RIGHT);
moveAtSpeed(MEDIUM,MEDIUM);
}
}
if (movement == TURNLEFT)
{
if (escape_state == ROTATE || avoid_state == ROTATE)
{
stop();
rotate(MEDIUM, LEFT, 90, BLOCKING);
escape_state = FREE;
avoid_state = FREE;
cruise_state = READY;
}
else
{
changeDirection(LEFT);
moveAtSpeed(MEDIUM,MEDIUM);
}
}
if (movement == UTURN)
{
stop();
rotate(MEDIUM, RIGHT, 180, BLOCKING);
}
if (movement == FORWARD)
moveAtSpeed(CRUISE,CRUISE);
if (movement == HARDRIGHT)
moveAtSpeed(FAST,SLOW);
if (movement == HARDLEFT)
moveAtSpeed(SLOW,FAST);
if (movement == RIGHT)
moveAtSpeed(FAST,MEDIUM);
if (movement == LEFT)
moveAtSpeed(MEDIUM,FAST);
if (movement == STOP)
stop();
if (movement == BACKWARD)
{
changeDirection(BWD);
moveAtSpeed(MEDIUM,MEDIUM);
}
}
void acsStateChanged(void) // displays the ACS state on the LEDs 2+3+5+6
{
if (obstacle_left && obstacle_right)
{
statusLEDs.LED6 = true;
statusLEDs.LED3 = true;
}
else
{
statusLEDs.LED6 = false;
statusLEDs.LED3 = false;
}
statusLEDs.LED5 = obstacle_left;
statusLEDs.LED2 = obstacle_right;
updateStatusLEDs();
}
void task_expansion(void) // runs background funktions while using infinite loops
{
frontlight();
I2C_dataupload();
}
void frontlight(void) // front diodes will be switched on in darkness
{
if (adcLSL<450)
{
PORTA |= ADC0;
}
if (adcLSL>680)
{
PORTA &= ~ADC0;
}
if (adcLSR<450)
{
PORTA |= ADC1;
}
if (adcLSR>700)
{
PORTA &= ~ADC1;
}
}
/*****************************************************************************/
// Main
int main(void)
{
initRobotBase();
powerON();
setLEDs(0b111111);
mSleep(500);
setLEDs(0b000000);
mSleep(500);
setLEDs(0b111111);
setACSPwrHigh(); // adjust ACS to Low Med or High depending on the circumstances
I2CTWI_initMaster(100); // I2C speed set to 100kHz
DDRA |= (ADC0 | ADC1); // definition of ADC0, ADC1 as output-channels
ACS_setStateChangedHandler(acsStateChanged); // Set ACS state changed event handler
// Main loop
while(true)
{
task_RP6System();
task_expansion();
if(move_enable == true)
movecontroller();
else
stop();
}
return 0;
}
Zitieren
Lesezeichen