1359 lines
30 KiB
C
1359 lines
30 KiB
C
/*================================================================================================
|
||
|
||
CMtec RoboMSI (MasterSlaveInterface) copyright 2005 v1.0 (2006-02-21)
|
||
|
||
Name: Christoffer Martinsson
|
||
E-mail: cm@cmtec.se
|
||
|
||
|
||
================================================================================================*/
|
||
//#define F_CPU 8000000UL //internal 8MHz
|
||
//#define F_CPU 16000000UL
|
||
|
||
#include <util/delay.h>
|
||
#include <string.h>
|
||
#include <inttypes.h>
|
||
#include <avr/io.h>
|
||
#include <avr/interrupt.h>
|
||
#include <avr/eeprom.h>
|
||
#include <avr/pgmspace.h>
|
||
|
||
#include <lcdDOG.h>
|
||
#include <rfID.h>
|
||
#include <roboMSP.h>
|
||
#include <ping.h>
|
||
|
||
// Time defenitions for servo generator
|
||
#define SERVO_TIMER 0xEE
|
||
#define SERVO_POS_RIGHT_90 67 //31
|
||
#define SERVO_POS_RIGHT_45 103 //49
|
||
#define SERVO_POS_CENTER 140 //68
|
||
#define SERVO_POS_LEFT_45 174 //86
|
||
#define SERVO_POS_LEFT_90 208 //104
|
||
|
||
#define SERVO_POS_MOTOR_STOP 119 //68
|
||
#define SERVO_POS_MOTOR_FW 133//130 //68
|
||
#define SERVO_POS_MOTOR_RE 108//111 //68
|
||
#define SERVO_POS_MOTOR_FW_SOFT 127 //68
|
||
#define SERVO_POS_MOTOR_RE_SOFT 114 //68
|
||
#define SERVO_POS_MOTOR_FW_FAST 134//139//133 //68
|
||
#define SERVO_POS_MOTOR_RE_FAST 104//105//108 //68
|
||
|
||
// Formula = X * 13 (X = distance in mm)
|
||
#define DISTANCE_TO_STOP 5500UL
|
||
#define DISTANCE_ONE_METER 6000UL
|
||
#define DISTANCE_TWO_METER 26400UL
|
||
|
||
// Port defenitions for servo generator
|
||
#define SERVO_PING PC1
|
||
#define SERVO_MOTOR_LEFT PC0
|
||
#define SERVO_MOTOR_RIGHT PC2
|
||
#define SERVO_PORT PORTC
|
||
// Variables for servo generator
|
||
uint8_t servoPing = SERVO_POS_CENTER;
|
||
uint8_t servoMotorLeft = SERVO_POS_MOTOR_STOP;
|
||
uint8_t servoMotorRight = SERVO_POS_MOTOR_STOP;
|
||
uint8_t servoCounter = 0;
|
||
|
||
uint8_t servoMotorLeftReg = SERVO_POS_MOTOR_STOP;
|
||
uint8_t servoMotorRightReg = SERVO_POS_MOTOR_STOP;
|
||
|
||
// Time definitions for Program Timer
|
||
#define PROG_TIMER 0xBFFF // 14ms //0xB1DF // 20ms
|
||
// Variables for Program Timer
|
||
uint8_t progCounterPing = 0;
|
||
uint8_t progCounterPPM = 0;
|
||
|
||
uint8_t progCounter = 0;
|
||
uint8_t progTimer = 0;
|
||
uint8_t progPosition = 0;
|
||
uint8_t progDoUpdate = 0;
|
||
|
||
uint16_t tempDistance = 0;
|
||
uint16_t tempDistanceToWall = 0;
|
||
uint16_t tempLastDistanceToWall = 0;
|
||
uint16_t tempLeftDistance = 0;
|
||
uint16_t tempRightDistance = 0;
|
||
|
||
// Variables for main program
|
||
uint8_t roboActive = 0;
|
||
uint8_t rfEnable = 0;
|
||
|
||
uint8_t switchOneCheck = 0;
|
||
uint8_t switchTwoCheck = 0;
|
||
|
||
uint8_t regulatorMode = 0;
|
||
uint8_t regulatorTimer = 0;
|
||
uint8_t regulatorCounter = 0;
|
||
uint8_t regulatorMeasureCounter = 0;
|
||
|
||
const char menuTable[][49] PROGMEM =
|
||
{
|
||
" CMtec "
|
||
"roboCrawler v1.1"
|
||
" ",
|
||
|
||
" mode 1: "
|
||
" Follow wall at "
|
||
" a distance(1m) ",
|
||
|
||
" mode 2: "
|
||
" Drive and void "
|
||
" any walls ",
|
||
|
||
" mode 3: "
|
||
"Measure distance"
|
||
" forward ",
|
||
|
||
" mode RF: "
|
||
" RF actived and "
|
||
"waiting for com.",
|
||
};
|
||
|
||
void checkCommunication(void);
|
||
void showDistanceOnLCD(uint16_t distance, uint8_t mode, uint8_t position);
|
||
/*================================================================================================
|
||
Functions
|
||
================================================================================================*/
|
||
/*------------------------------------------------------------------------------------------------
|
||
initIO
|
||
|
||
Description: Initialise I/O
|
||
Input: -
|
||
Output: -
|
||
------------------------------------------------------------------------------------------------*/
|
||
void initIO (void){
|
||
|
||
TCCR1B = (1<<CS11); // Timer1 prescaler = fc/8
|
||
|
||
TCNT1H = (uint8_t)(PROG_TIMER>>8);
|
||
TCNT1L = (uint8_t)(PROG_TIMER);
|
||
|
||
TCNT2 = SERVO_TIMER;
|
||
|
||
TIMSK |= (1<<TOIE1)|(1<<TOIE2); // Timer1 interrupt enable
|
||
DDRC |= (1<<PC0)|(1<<PC1)|(1<<PC2);
|
||
|
||
DDRA &= ~((1<<PA4)|(1<<PA5)|(1<<PA6)|(1<<PA7)); // Configuration button pins as input
|
||
PORTA |= (1<<PA4)|(1<<PA5)|(1<<PA6)|(1<<PA7); // Pull-up on configuration button pins
|
||
|
||
progPosition = 0;
|
||
}
|
||
/*================================================================================================
|
||
Interrupt
|
||
================================================================================================*/
|
||
/*------------------------------------------------------------------------------------------------
|
||
SIG_OVERFLOW1
|
||
|
||
Description: Timer1 overflow interrupt. Used as program timer
|
||
Input: -
|
||
Output: -
|
||
------------------------------------------------------------------------------------------------*/
|
||
ISR (TIMER1_OVF_vect)
|
||
{
|
||
// Program for update PPM pulses to all servos
|
||
if (++progCounterPPM >= 2)
|
||
{
|
||
// Start for servo PPM pulses
|
||
servoCounter = 0;
|
||
SERVO_PORT |= (1<<SERVO_PING)|(1<<SERVO_MOTOR_LEFT)|(1<<SERVO_MOTOR_RIGHT);
|
||
TCCR2 |= (1<<CS21); // Timer2 start (prescaler = fc/8)
|
||
progCounterPPM = 0;
|
||
}
|
||
|
||
// Trigg program routine
|
||
progDoUpdate = 1;
|
||
|
||
// Set timer value
|
||
TCNT1H = (uint8_t)(PROG_TIMER>>8);
|
||
TCNT1L = (uint8_t)(PROG_TIMER);
|
||
}
|
||
/*------------------------------------------------------------------------------------------------
|
||
SIG_OVERFLOW2
|
||
|
||
Description: Timer1 overflow interrupt. Used as servo (PPM) timer
|
||
Input: -
|
||
Output: -
|
||
------------------------------------------------------------------------------------------------*/
|
||
ISR (TIMER2_OVF_vect)
|
||
{
|
||
uint8_t done = 1;
|
||
|
||
// Check each servo output if pulselength is reached
|
||
if(servoCounter >= servoPing)
|
||
SERVO_PORT &= ~(1<<SERVO_PING);
|
||
else
|
||
{
|
||
done = 0;
|
||
asm volatile("NOP"); // Correction for constant interrupt length
|
||
}
|
||
|
||
if(servoCounter >= servoMotorLeft)
|
||
SERVO_PORT &= ~(1<<SERVO_MOTOR_LEFT);
|
||
else
|
||
{
|
||
done = 0;
|
||
asm volatile("NOP"); // Correction for constant interrupt length
|
||
}
|
||
|
||
if(servoCounter >= servoMotorRight)
|
||
SERVO_PORT &= ~(1<<SERVO_MOTOR_RIGHT);
|
||
else
|
||
{
|
||
done = 0;
|
||
asm volatile("NOP"); // Correction for constant interrupt length
|
||
}
|
||
|
||
if(done)
|
||
TCCR2 &= ~(1<<CS21); // Timer1 stop
|
||
else
|
||
{
|
||
servoCounter++;
|
||
asm volatile("NOP"); // Correction for constant interrupt length
|
||
asm volatile("NOP");
|
||
}
|
||
TCNT2 = SERVO_TIMER;
|
||
}
|
||
/*------------------------------------------------------------------------------------------------
|
||
activateRobot
|
||
|
||
Description:
|
||
Input: -
|
||
Output: -
|
||
------------------------------------------------------------------------------------------------*/
|
||
void activateRobot(uint8_t mode)
|
||
{
|
||
lcdSetLayout(0);
|
||
lcdWriteStringP(menuTable[mode]);
|
||
|
||
progTimer = 200;
|
||
|
||
roboActive = mode;
|
||
}
|
||
/*------------------------------------------------------------------------------------------------
|
||
deactivateRobot
|
||
|
||
Description:
|
||
Input: -
|
||
Output: -
|
||
------------------------------------------------------------------------------------------------*/
|
||
void deactivateRobot(void)
|
||
{
|
||
roboActive = 0;
|
||
|
||
servoMotorLeft = SERVO_POS_MOTOR_STOP;
|
||
servoMotorRight = SERVO_POS_MOTOR_STOP;
|
||
servoPing = SERVO_POS_CENTER;
|
||
|
||
rfIdDisable();
|
||
}
|
||
|
||
|
||
/*------------------------------------------------------------------------------------------------
|
||
updateProgram
|
||
|
||
Description:
|
||
Input: -
|
||
Output: -
|
||
------------------------------------------------------------------------------------------------*/
|
||
void updateProgram(void)
|
||
{
|
||
// Check radio communication
|
||
if(rfEnable)
|
||
checkCommunication();
|
||
|
||
// Program for robot AI
|
||
if (roboActive == 1)
|
||
{
|
||
// Program(2) for random RFID serach
|
||
if (++progCounter >= progTimer)
|
||
{
|
||
switch (progPosition)
|
||
{
|
||
case 0:
|
||
// K<>r framm<6D>t
|
||
lcdClearDisplay();
|
||
lcdSetLayout(1);
|
||
lcdSetPos(0x00);
|
||
lcdWriteString("Drive: FORWARD ");
|
||
|
||
servoMotorLeft = SERVO_POS_MOTOR_FW_FAST;
|
||
servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
|
||
|
||
progTimer = 20;
|
||
progPosition++;
|
||
|
||
break;
|
||
|
||
case 1:
|
||
// K<>r framm<6D>t till dess att avst<73>ndet mot v<>ggen <20>r 1meter
|
||
servoMotorLeft = SERVO_POS_MOTOR_FW_SOFT;
|
||
servoMotorRight = SERVO_POS_MOTOR_FW_SOFT;
|
||
|
||
pingSendPing();
|
||
progTimer = 4;
|
||
|
||
if(pingGetReady())
|
||
{
|
||
|
||
tempDistance = pingGetDistance();
|
||
|
||
if(tempDistance < DISTANCE_ONE_METER)
|
||
{
|
||
showDistanceOnLCD(0,0,2);
|
||
progTimer = 1;
|
||
progPosition++;
|
||
}
|
||
else
|
||
{
|
||
showDistanceOnLCD(tempDistance-DISTANCE_ONE_METER,0,2);
|
||
}
|
||
}
|
||
|
||
break;
|
||
|
||
case 2:
|
||
// Bromsa (backa n<>got)
|
||
lcdSetPos(0x00);
|
||
lcdWriteString("Drive: STOP ");
|
||
// Stop and breake
|
||
servoMotorLeft = SERVO_POS_MOTOR_STOP;
|
||
servoMotorRight = SERVO_POS_MOTOR_STOP;
|
||
servoMotorLeft = SERVO_POS_MOTOR_RE;
|
||
servoMotorRight = SERVO_POS_MOTOR_RE;
|
||
|
||
progTimer = 40;
|
||
progPosition++;
|
||
|
||
break;
|
||
|
||
case 3:
|
||
// Stanna
|
||
|
||
servoMotorLeft = SERVO_POS_MOTOR_STOP;
|
||
servoMotorRight = SERVO_POS_MOTOR_STOP;
|
||
|
||
progTimer = 1;
|
||
regulatorMeasureCounter = 0;
|
||
progPosition++;
|
||
|
||
break;
|
||
|
||
case 4:
|
||
// Vrid servo 45grader v<>nster (fr<66>n centrum)
|
||
servoPing = SERVO_POS_LEFT_45;
|
||
progTimer = 20;
|
||
progPosition++;
|
||
|
||
break;
|
||
|
||
case 5:
|
||
// M<>t och spara avst<73>nd
|
||
pingSendPing();
|
||
progTimer = 4;
|
||
|
||
if(pingGetReady())
|
||
{
|
||
tempLeftDistance = pingGetDistance();
|
||
progTimer = 1;
|
||
progPosition++;
|
||
}
|
||
|
||
break;
|
||
|
||
case 6:
|
||
// Vrid servo 45grader h<>ger (fr<66>n centrum)
|
||
servoPing = SERVO_POS_RIGHT_45;
|
||
progTimer = 40;
|
||
progPosition++;
|
||
|
||
break;
|
||
|
||
case 7:
|
||
// M<>t och spara avst<73>nd
|
||
pingSendPing();
|
||
progTimer = 4;
|
||
|
||
if(pingGetReady())
|
||
{
|
||
tempRightDistance = pingGetDistance();
|
||
progTimer = 1;
|
||
progPosition++;
|
||
}
|
||
|
||
break;
|
||
|
||
case 8:
|
||
|
||
servoPing = SERVO_POS_CENTER;
|
||
|
||
if(tempLeftDistance > tempRightDistance)
|
||
{
|
||
regulatorMode = 0; // M<>ter <20>t h<>ger
|
||
}
|
||
else
|
||
{
|
||
regulatorMode = 1; // M<>ter <20>t v<>nster
|
||
}
|
||
|
||
progTimer = 50;
|
||
progPosition++;
|
||
|
||
break;
|
||
|
||
case 9:
|
||
// M<>t och spara avst<73>nd
|
||
pingSendPing();
|
||
progTimer = 4;
|
||
|
||
if(pingGetReady())
|
||
{
|
||
lcdSetPos(0x00);
|
||
lcdWriteString("Align to wall ");
|
||
tempLastDistanceToWall = pingGetDistance();
|
||
tempDistanceToWall = tempLastDistanceToWall;
|
||
progTimer = 1;
|
||
progPosition++;
|
||
}
|
||
|
||
break;
|
||
|
||
case 10:
|
||
// Reglera in sig vinker<65>t mot v<>ggen
|
||
// Vrider sig ett steg
|
||
if (regulatorMode == 0) // Tittar h<>ger
|
||
{
|
||
// sv<73>ng h<>ger
|
||
servoMotorLeft = SERVO_POS_MOTOR_FW_FAST;
|
||
servoMotorRight = SERVO_POS_MOTOR_RE_FAST;
|
||
progTimer = 7;
|
||
}
|
||
else if (regulatorMode == 1) // Tittar v<>nster
|
||
{
|
||
// sv<73>ng v<>nster
|
||
servoMotorLeft = SERVO_POS_MOTOR_RE_FAST;
|
||
servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
|
||
progTimer = 7;
|
||
}
|
||
|
||
progPosition++;
|
||
|
||
break;
|
||
|
||
case 11:
|
||
// Reglera in sig vinker<65>t mot v<>ggen
|
||
// Kontrollerar avst<73>nd
|
||
pingSendPing();
|
||
progTimer = 4;
|
||
|
||
servoMotorLeft = SERVO_POS_MOTOR_STOP;
|
||
servoMotorRight = SERVO_POS_MOTOR_STOP;
|
||
|
||
if(pingGetReady())
|
||
{
|
||
|
||
tempDistance = pingGetDistance();
|
||
|
||
if(tempDistance > tempLastDistanceToWall)
|
||
{
|
||
lcdSetPos(0x00);
|
||
lcdWriteString(" ^-- (1m) --^ ");
|
||
|
||
progTimer = 1;
|
||
progPosition++;
|
||
}
|
||
else
|
||
progPosition--;
|
||
|
||
tempLastDistanceToWall = tempDistance;
|
||
}
|
||
|
||
break;
|
||
|
||
case 12:
|
||
// Reglera in position 1meter fr<66>n v<>ggen
|
||
pingSendPing();
|
||
progTimer = 4;
|
||
|
||
if(pingGetReady())
|
||
{
|
||
|
||
tempDistance = pingGetDistance();
|
||
|
||
if(tempDistance > DISTANCE_ONE_METER)
|
||
showDistanceOnLCD((tempDistance-DISTANCE_ONE_METER),0,2);
|
||
else
|
||
showDistanceOnLCD((DISTANCE_ONE_METER-tempDistance),1,2);
|
||
|
||
if(tempDistance < (DISTANCE_ONE_METER - 200))
|
||
{
|
||
// K<>r framm<6D>t (<28>ka hastigheten l<>ngsamt)
|
||
if(servoMotorLeft > SERVO_POS_MOTOR_RE_SOFT)
|
||
{
|
||
servoMotorLeft--;
|
||
servoMotorRight--;
|
||
}
|
||
|
||
regulatorMeasureCounter = 0;
|
||
}
|
||
else if(tempDistance > (DISTANCE_ONE_METER + 200))
|
||
{
|
||
// K<>r bak<61>t (<28>ka hastigheten l<>ngsamt)
|
||
if(servoMotorLeft < SERVO_POS_MOTOR_FW_SOFT)
|
||
{
|
||
servoMotorLeft++;
|
||
servoMotorRight++;
|
||
|
||
}
|
||
|
||
regulatorMeasureCounter = 0;
|
||
}
|
||
else
|
||
{
|
||
servoMotorLeft = SERVO_POS_MOTOR_STOP;
|
||
servoMotorRight = SERVO_POS_MOTOR_STOP;
|
||
|
||
if(++regulatorMeasureCounter == 5)
|
||
{
|
||
lcdSetPos(0x00);
|
||
lcdWriteString("Posistion locked");
|
||
|
||
servoMotorLeft = SERVO_POS_MOTOR_STOP;
|
||
servoMotorRight = SERVO_POS_MOTOR_STOP;
|
||
tempDistanceToWall = tempDistance;
|
||
progTimer = 50;
|
||
regulatorMeasureCounter = 0;
|
||
progPosition++;
|
||
}
|
||
}
|
||
}
|
||
|
||
break;
|
||
|
||
case 13:
|
||
// Vrider 90grader
|
||
servoPing = SERVO_POS_CENTER;
|
||
|
||
if(tempLeftDistance > tempRightDistance)
|
||
{
|
||
lcdSetPos(0x00);
|
||
lcdWriteString("Drive: Turn < ");
|
||
|
||
servoMotorLeft = SERVO_POS_MOTOR_RE_FAST;
|
||
servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
|
||
servoPing = SERVO_POS_RIGHT_90;
|
||
regulatorMode = 0; // M<>ter <20>t h<>ger
|
||
progTimer = 50;
|
||
}
|
||
else
|
||
{
|
||
lcdSetPos(0x00);
|
||
lcdWriteString("Drive: Turn > ");
|
||
|
||
servoMotorLeft = SERVO_POS_MOTOR_FW_FAST;
|
||
servoMotorRight = SERVO_POS_MOTOR_RE_FAST;
|
||
servoPing = SERVO_POS_LEFT_90;
|
||
regulatorMode = 1; // M<>ter <20>t v<>nster
|
||
progTimer = 50;
|
||
}
|
||
|
||
progPosition++;
|
||
|
||
break;
|
||
|
||
case 14:
|
||
// Stanna
|
||
servoMotorLeft = SERVO_POS_MOTOR_STOP;
|
||
servoMotorRight = SERVO_POS_MOTOR_STOP;
|
||
|
||
progTimer = 10;
|
||
regulatorTimer = 2;
|
||
progPosition++;
|
||
progPosition++;
|
||
progPosition++;
|
||
|
||
break;
|
||
|
||
case 15:
|
||
// K<>r framm<6D>t
|
||
lcdSetPos(0x00);
|
||
lcdWriteString("Drive: FORWARD ");
|
||
|
||
servoMotorLeft = SERVO_POS_MOTOR_FW_FAST;
|
||
servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
|
||
|
||
progTimer = 20;
|
||
progPosition++;
|
||
|
||
break;
|
||
|
||
case 16:
|
||
// K<>r framm<6D>t
|
||
servoMotorLeft = SERVO_POS_MOTOR_FW_SOFT;
|
||
servoMotorRight = SERVO_POS_MOTOR_FW_SOFT;
|
||
|
||
progTimer = 10;
|
||
progPosition++;
|
||
|
||
break;
|
||
|
||
case 17:
|
||
// M<>ter avst<73>nd
|
||
pingSendPing();
|
||
progTimer = 3;
|
||
|
||
if(pingGetReady())
|
||
{
|
||
|
||
tempDistance = pingGetDistance();
|
||
progTimer = 1;
|
||
progPosition++;
|
||
|
||
}
|
||
|
||
break;
|
||
|
||
case 18:
|
||
// Reglerar 1meter fr<66>n v<>ggen
|
||
if(++regulatorCounter >= regulatorTimer)
|
||
{
|
||
progPosition++;
|
||
progTimer = 1;
|
||
regulatorCounter = 0;
|
||
}
|
||
else
|
||
{
|
||
uint8_t calculateProgTimer = 0;
|
||
if (regulatorMode == 0) // Tittar h<>ger
|
||
{
|
||
lcdSetPos(0x00);
|
||
lcdWriteString(" ]--- (1m) --->|");
|
||
|
||
if(tempDistance > DISTANCE_ONE_METER)
|
||
showDistanceOnLCD((tempDistance-DISTANCE_ONE_METER),3,2);
|
||
else
|
||
showDistanceOnLCD((DISTANCE_ONE_METER-tempDistance),2,2);
|
||
|
||
if((tempDistance < tempLastDistanceToWall) && (tempDistance < (DISTANCE_ONE_METER -1000)))
|
||
{
|
||
// sv<73>ng v<>nster
|
||
servoMotorLeft = SERVO_POS_MOTOR_RE_FAST;
|
||
servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
|
||
|
||
calculateProgTimer = ((DISTANCE_ONE_METER - tempDistance)/40);
|
||
|
||
if(calculateProgTimer < 15)
|
||
progTimer = calculateProgTimer;
|
||
else
|
||
progTimer = 15;
|
||
|
||
}
|
||
else if((tempDistance < tempLastDistanceToWall) && (tempDistance < (DISTANCE_ONE_METER -10)))
|
||
{
|
||
// sv<73>ng v<>nster
|
||
servoMotorLeft = SERVO_POS_MOTOR_RE_FAST;
|
||
servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
|
||
|
||
progTimer = 5;
|
||
|
||
}
|
||
else if((tempDistance > tempLastDistanceToWall) && (tempDistance > (DISTANCE_ONE_METER+1000)))
|
||
{
|
||
// sv<73>ng h<>ger
|
||
servoMotorLeft = SERVO_POS_MOTOR_FW_FAST;
|
||
servoMotorRight = SERVO_POS_MOTOR_RE_FAST;
|
||
|
||
calculateProgTimer = ((tempDistance - DISTANCE_ONE_METER)/40);
|
||
|
||
if(calculateProgTimer < 15)
|
||
progTimer = calculateProgTimer;
|
||
else
|
||
progTimer = 15;
|
||
|
||
}
|
||
else if((tempDistance > tempLastDistanceToWall) && (tempDistance > (DISTANCE_ONE_METER+10)))
|
||
{
|
||
// sv<73>ng h<>ger
|
||
servoMotorLeft = SERVO_POS_MOTOR_FW_FAST;
|
||
servoMotorRight = SERVO_POS_MOTOR_RE_FAST;
|
||
|
||
progTimer = 5;
|
||
|
||
}
|
||
else
|
||
{
|
||
servoMotorLeft = SERVO_POS_MOTOR_FW_SOFT;
|
||
servoMotorRight = SERVO_POS_MOTOR_FW_SOFT;
|
||
progTimer = 4;
|
||
}
|
||
}
|
||
|
||
else if (regulatorMode == 1) // Tittar v<>nster
|
||
{
|
||
lcdSetPos(0x00);
|
||
lcdWriteString("|<--- (1m) ---[ ");
|
||
|
||
if(tempDistance > DISTANCE_ONE_METER)
|
||
showDistanceOnLCD((tempDistance-DISTANCE_ONE_METER),2,2);
|
||
else
|
||
showDistanceOnLCD((DISTANCE_ONE_METER-tempDistance),3,2);
|
||
|
||
if((tempDistance < tempLastDistanceToWall) && (tempDistance < (DISTANCE_ONE_METER-1000)))
|
||
{
|
||
// sv<73>ng h<>ger
|
||
servoMotorLeft = SERVO_POS_MOTOR_FW_FAST;
|
||
servoMotorRight = SERVO_POS_MOTOR_RE_FAST;
|
||
|
||
calculateProgTimer = ((DISTANCE_ONE_METER - tempDistance)/40);
|
||
|
||
if(calculateProgTimer < 15)
|
||
progTimer = calculateProgTimer;
|
||
else
|
||
progTimer = 15;
|
||
|
||
}
|
||
else if((tempDistance < tempLastDistanceToWall) && (tempDistance < (DISTANCE_ONE_METER-10)))
|
||
{
|
||
// sv<73>ng h<>ger
|
||
servoMotorLeft = SERVO_POS_MOTOR_FW_FAST;
|
||
servoMotorRight = SERVO_POS_MOTOR_RE_FAST;
|
||
|
||
progTimer = 5;
|
||
|
||
}
|
||
else if((tempDistance > tempLastDistanceToWall) && (tempDistance > (DISTANCE_ONE_METER +1000)))
|
||
{
|
||
// sv<73>ng v<>nster
|
||
servoMotorLeft = SERVO_POS_MOTOR_RE_FAST;
|
||
servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
|
||
|
||
calculateProgTimer = ((tempDistance - DISTANCE_ONE_METER)/40);
|
||
|
||
if(calculateProgTimer < 15)
|
||
progTimer = calculateProgTimer;
|
||
else
|
||
progTimer = 15;
|
||
}
|
||
else if((tempDistance > tempLastDistanceToWall) && (tempDistance > (DISTANCE_ONE_METER +10)))
|
||
{
|
||
// sv<73>ng v<>nster
|
||
servoMotorLeft = SERVO_POS_MOTOR_RE_FAST;
|
||
servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
|
||
|
||
progTimer = 5;
|
||
}
|
||
else
|
||
{
|
||
servoMotorLeft = SERVO_POS_MOTOR_FW_SOFT;
|
||
servoMotorRight = SERVO_POS_MOTOR_FW_SOFT;
|
||
progTimer = 4;
|
||
}
|
||
}
|
||
tempLastDistanceToWall = tempDistance;
|
||
progPosition--;
|
||
progPosition--;
|
||
}
|
||
|
||
break;
|
||
|
||
case 19:
|
||
|
||
servoPing = SERVO_POS_CENTER;
|
||
|
||
servoMotorLeft = SERVO_POS_MOTOR_STOP;
|
||
servoMotorRight = SERVO_POS_MOTOR_STOP;
|
||
|
||
progTimer = 35;
|
||
progPosition++;
|
||
|
||
break;
|
||
|
||
case 20:
|
||
|
||
progTimer = 3;
|
||
pingSendPing();
|
||
|
||
if(pingGetReady())
|
||
{
|
||
|
||
tempDistance = pingGetDistance();
|
||
|
||
if(tempDistance < DISTANCE_ONE_METER)
|
||
{
|
||
showDistanceOnLCD(0,0,1);
|
||
progPosition = 2;
|
||
}
|
||
else if (tempDistance < (DISTANCE_ONE_METER*2))
|
||
{
|
||
showDistanceOnLCD(tempDistance-DISTANCE_ONE_METER,0,1);
|
||
regulatorTimer = 2;
|
||
progPosition++;
|
||
}
|
||
else
|
||
{
|
||
showDistanceOnLCD(tempDistance-DISTANCE_ONE_METER,0,1);
|
||
regulatorTimer = 10;
|
||
progPosition++;
|
||
}
|
||
}
|
||
|
||
break;
|
||
|
||
case 21:
|
||
|
||
if (regulatorMode == 0) // Tittar h<>ger
|
||
{
|
||
servoPing = SERVO_POS_RIGHT_90;
|
||
}
|
||
else
|
||
{
|
||
servoPing = SERVO_POS_LEFT_90;
|
||
}
|
||
|
||
progTimer = 5;
|
||
progPosition = 15;
|
||
|
||
break;
|
||
|
||
}
|
||
|
||
progCounter = 0;
|
||
}
|
||
}
|
||
|
||
// Program for robot AI
|
||
if (roboActive == 2)
|
||
{
|
||
// Program(2) for random RFID serach
|
||
if (++progCounter >= progTimer)
|
||
{
|
||
|
||
switch (progPosition)
|
||
{
|
||
case 0:
|
||
|
||
lcdClearDisplay();
|
||
lcdSetLayout(1);
|
||
|
||
progTimer = 1;
|
||
progPosition++;
|
||
|
||
break;
|
||
|
||
case 1:
|
||
|
||
lcdSetPos(0x00);
|
||
lcdWriteString("Drive: FORWARD ");
|
||
|
||
servoMotorLeft = SERVO_POS_MOTOR_FW_FAST;
|
||
servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
|
||
|
||
progTimer = 20;
|
||
progPosition++;
|
||
|
||
break;
|
||
|
||
case 2:
|
||
|
||
servoMotorLeft = SERVO_POS_MOTOR_FW_SOFT;
|
||
servoMotorRight = SERVO_POS_MOTOR_FW_SOFT;
|
||
|
||
pingSendPing();
|
||
progTimer = 3;
|
||
|
||
if(pingGetReady())
|
||
{
|
||
|
||
tempDistance = pingGetDistance();
|
||
|
||
if(tempDistance < DISTANCE_TO_STOP)
|
||
{
|
||
showDistanceOnLCD(0,0,2);
|
||
progPosition++;
|
||
}
|
||
else
|
||
{
|
||
showDistanceOnLCD(tempDistance-DISTANCE_TO_STOP,0,2);
|
||
}
|
||
|
||
}
|
||
|
||
break;
|
||
|
||
case 3:
|
||
|
||
lcdSetPos(0x00);
|
||
lcdWriteString("Drive: STOP ");
|
||
|
||
// Stop and breake
|
||
servoMotorLeft = SERVO_POS_MOTOR_STOP;
|
||
servoMotorRight = SERVO_POS_MOTOR_STOP;
|
||
servoMotorLeft = SERVO_POS_MOTOR_RE;
|
||
servoMotorRight = SERVO_POS_MOTOR_RE;
|
||
|
||
progTimer = 50;
|
||
|
||
progPosition++;
|
||
|
||
break;
|
||
|
||
case 4:
|
||
|
||
servoMotorLeft = SERVO_POS_MOTOR_STOP;
|
||
servoMotorRight = SERVO_POS_MOTOR_STOP;
|
||
|
||
progTimer = 2;
|
||
|
||
progPosition++;
|
||
|
||
break;
|
||
|
||
case 5:
|
||
|
||
servoPing = SERVO_POS_LEFT_45;
|
||
|
||
progTimer = 20;
|
||
|
||
progPosition++;
|
||
|
||
break;
|
||
|
||
case 6:
|
||
|
||
pingSendPing();
|
||
progTimer = 4;
|
||
|
||
if(pingGetReady())
|
||
{
|
||
tempLeftDistance = pingGetDistance();
|
||
progTimer = 20;
|
||
progPosition++;
|
||
}
|
||
|
||
|
||
break;
|
||
|
||
case 7:
|
||
|
||
servoPing = SERVO_POS_RIGHT_45;
|
||
|
||
progTimer = 40;
|
||
|
||
progPosition++;
|
||
|
||
break;
|
||
|
||
case 8:
|
||
|
||
pingSendPing();
|
||
progTimer = 4;
|
||
|
||
if(pingGetReady())
|
||
{
|
||
tempRightDistance = pingGetDistance();
|
||
|
||
servoPing = SERVO_POS_CENTER;
|
||
|
||
if((tempLeftDistance < DISTANCE_TO_STOP )
|
||
&& (tempRightDistance < DISTANCE_TO_STOP))
|
||
{
|
||
lcdSetPos(0x00);
|
||
lcdWriteString("Drive: Turn <<< ");
|
||
|
||
servoMotorLeft = SERVO_POS_MOTOR_RE_FAST;
|
||
servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
|
||
progTimer = 70;
|
||
}
|
||
else if(tempLeftDistance > tempRightDistance)
|
||
{
|
||
lcdSetPos(0x00);
|
||
lcdWriteString("Drive: Turn < ");
|
||
|
||
servoMotorLeft = SERVO_POS_MOTOR_RE_FAST;
|
||
servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
|
||
progTimer = 40;
|
||
}
|
||
else
|
||
{
|
||
lcdSetPos(0x00);
|
||
lcdWriteString("Drive: Turn > ");
|
||
|
||
servoMotorLeft = SERVO_POS_MOTOR_FW_FAST;
|
||
servoMotorRight = SERVO_POS_MOTOR_RE_FAST;
|
||
progTimer = 40;
|
||
}
|
||
|
||
progPosition++;
|
||
}
|
||
|
||
break;
|
||
|
||
case 9:
|
||
|
||
servoMotorLeft = SERVO_POS_MOTOR_STOP;
|
||
servoMotorRight = SERVO_POS_MOTOR_STOP;
|
||
|
||
progTimer = 10;
|
||
progPosition = 1;
|
||
|
||
break;
|
||
}
|
||
|
||
progCounter = 0;
|
||
}
|
||
}
|
||
|
||
if (roboActive == 3)
|
||
{
|
||
// Program(2) for random RFID serach
|
||
if (++progCounter >= progTimer)
|
||
{
|
||
|
||
switch (progPosition)
|
||
{
|
||
case 0:
|
||
|
||
lcdClearDisplay();
|
||
lcdSetLayout(1);
|
||
lcdSetPos(0x00);
|
||
lcdWriteString("Servo: forward ");
|
||
lcdSetPos(0x10);
|
||
lcdWriteString("Distance: ");
|
||
|
||
progTimer = 1;
|
||
progPosition++;
|
||
|
||
break;
|
||
|
||
case 1:
|
||
|
||
pingSendPing();
|
||
progTimer = 10;
|
||
|
||
if(pingGetReady())
|
||
{
|
||
|
||
tempDistance = pingGetDistance();
|
||
lcdSetPos(0x1A);
|
||
lcdWriteHexAsDecimal(tempDistance);
|
||
|
||
}
|
||
|
||
break;
|
||
}
|
||
|
||
progCounter = 0;
|
||
}
|
||
}
|
||
}
|
||
/*------------------------------------------------------------------------------------------------
|
||
showDistanceOnLCD
|
||
|
||
Description:
|
||
Input: -
|
||
Output: -
|
||
------------------------------------------------------------------------------------------------*/
|
||
void showDistanceOnLCD(uint16_t distance, uint8_t mode, uint8_t position)
|
||
{
|
||
if(position == 1)
|
||
lcdSetPos(0x00);
|
||
else if(position == 2)
|
||
lcdSetPos(0x10);
|
||
else if(position == 3)
|
||
lcdSetPos(0x20);
|
||
|
||
if(mode == 0) // Both arrow pointing in
|
||
{
|
||
if(distance > 32000)
|
||
{
|
||
lcdWriteString(" ");
|
||
}
|
||
else if(distance > 16000)
|
||
{
|
||
lcdWriteString("> <");
|
||
}
|
||
else if(distance > 8000)
|
||
{
|
||
lcdWriteString(">> <<");
|
||
}
|
||
else if(distance > 4000)
|
||
{
|
||
lcdWriteString(">>> <<<");
|
||
}
|
||
else if(distance > 2000)
|
||
{
|
||
lcdWriteString(">>>> <<<<");
|
||
}
|
||
else if(distance > 1000)
|
||
{
|
||
lcdWriteString(">>>>> <<<<<");
|
||
}
|
||
else if(distance > 500)
|
||
{
|
||
lcdWriteString(">>>>>> <<<<<<");
|
||
}
|
||
else if(distance > 100)
|
||
{
|
||
lcdWriteString(">>>>>>> <<<<<<<");
|
||
}
|
||
else
|
||
{
|
||
lcdWriteString(">>>>>>>[]<<<<<<<");
|
||
}
|
||
}
|
||
else if(mode == 1) // Both arrow pointing Out
|
||
{
|
||
if(distance > 32000)
|
||
{
|
||
lcdWriteString(" ");
|
||
}
|
||
else if(distance > 16000)
|
||
{
|
||
lcdWriteString(" < > ");
|
||
}
|
||
else if(distance > 8000)
|
||
{
|
||
lcdWriteString(" << >> ");
|
||
}
|
||
else if(distance > 4000)
|
||
{
|
||
lcdWriteString(" <<< >>> ");
|
||
}
|
||
else if(distance > 2000)
|
||
{
|
||
lcdWriteString(" <<<< >>>> ");
|
||
}
|
||
else if(distance > 1000)
|
||
{
|
||
lcdWriteString(" <<<<< >>>>> ");
|
||
}
|
||
else if(distance > 500)
|
||
{
|
||
lcdWriteString(" <<<<<< >>>>>> ");
|
||
}
|
||
else if(distance > 100)
|
||
{
|
||
lcdWriteString("<<<<<<< >>>>>>>");
|
||
}
|
||
else
|
||
{
|
||
lcdWriteString("<<<<<<<[]>>>>>>>");
|
||
}
|
||
}
|
||
else if(mode == 2) // Left arrow pointing out.
|
||
{
|
||
if(distance > 6400)
|
||
{
|
||
lcdWriteString("<<<<<<< ");
|
||
}
|
||
else if(distance > 3200)
|
||
{
|
||
lcdWriteString(" <<<<<< ");
|
||
}
|
||
else if(distance > 1600)
|
||
{
|
||
lcdWriteString(" <<<<< ");
|
||
}
|
||
else if(distance > 800)
|
||
{
|
||
lcdWriteString(" <<<< ");
|
||
}
|
||
else if(distance > 400)
|
||
{
|
||
lcdWriteString(" <<< ");
|
||
}
|
||
else if(distance > 200)
|
||
{
|
||
lcdWriteString(" << ");
|
||
}
|
||
else if(distance > 30)
|
||
{
|
||
lcdWriteString(" < ");
|
||
}
|
||
else
|
||
{
|
||
lcdWriteString(" [] ");
|
||
}
|
||
}
|
||
else if(mode == 3) // Right arrow pointing out.
|
||
{
|
||
if(distance > 6400)
|
||
{
|
||
lcdWriteString(" >>>>>>>");
|
||
}
|
||
else if(distance > 3200)
|
||
{
|
||
lcdWriteString(" >>>>>> ");
|
||
}
|
||
else if(distance > 1600)
|
||
{
|
||
lcdWriteString(" >>>>> ");
|
||
}
|
||
else if(distance > 800)
|
||
{
|
||
lcdWriteString(" >>>> ");
|
||
}
|
||
else if(distance > 400)
|
||
{
|
||
lcdWriteString(" >>> ");
|
||
}
|
||
else if(distance > 200)
|
||
{
|
||
lcdWriteString(" >> ");
|
||
}
|
||
else if(distance > 30)
|
||
{
|
||
lcdWriteString(" > ");
|
||
}
|
||
else
|
||
{
|
||
lcdWriteString(" [] ");
|
||
}
|
||
}
|
||
}
|
||
/*------------------------------------------------------------------------------------------------
|
||
checkCommunication
|
||
|
||
Description: Check for incoming data packets.
|
||
Input: -
|
||
Output: -
|
||
------------------------------------------------------------------------------------------------*/
|
||
void checkCommunication(void){
|
||
|
||
|
||
if (rfIdGetTagPresent())
|
||
{
|
||
uint8_t *tempPointer = rfIdGetTag();
|
||
|
||
lcdSetPos(0x13);
|
||
lcdWriteString(tempPointer);
|
||
roboMSPSetData(tempPointer);
|
||
|
||
}
|
||
|
||
switch (roboMSPGetActiveStatus())
|
||
{
|
||
case ACTIVATE_P1:
|
||
activateRobot(1);
|
||
rfIdEnable();
|
||
break;
|
||
|
||
case ACTIVATE_P2:
|
||
activateRobot(2);
|
||
rfIdEnable();
|
||
break;
|
||
|
||
case DEACTIVATE:
|
||
deactivateRobot();
|
||
break;
|
||
|
||
case ACK:
|
||
rfIdClearBuffer();
|
||
break;
|
||
}
|
||
|
||
}
|
||
/*================================================================================================
|
||
Main
|
||
================================================================================================*/
|
||
int main (void){
|
||
|
||
cli(); // Disable global interrupt
|
||
|
||
initIO();
|
||
lcdInit();
|
||
rfIdInit();
|
||
roboMSPInit();
|
||
pingInit();
|
||
|
||
lcdSetIntensity(50);
|
||
lcdSetLayout(2);
|
||
lcdWriteStringP(menuTable[0]);
|
||
|
||
rfIdDisable();
|
||
roboMSPDisable();
|
||
|
||
sei(); // Enable global interrupt
|
||
|
||
while(1){
|
||
|
||
if(progDoUpdate)
|
||
{
|
||
progDoUpdate = 0;
|
||
updateProgram();
|
||
}
|
||
|
||
// Active-mode 1 (left switch lower position)
|
||
if (!bit_is_set(PINA,PA4))
|
||
{
|
||
if(switchOneCheck)
|
||
{
|
||
activateRobot(3);
|
||
switchOneCheck = 0;
|
||
}
|
||
}
|
||
// Active-mode 2 (left switch middle position)
|
||
else if ((bit_is_set(PINA,PA5)) && (bit_is_set(PINA,PA4)))
|
||
{
|
||
if(!switchOneCheck)
|
||
{
|
||
deactivateRobot();
|
||
rfEnable = 0;
|
||
switchOneCheck = 1;
|
||
main();
|
||
}
|
||
}
|
||
// Active-mode 3 (left switch upper position)
|
||
else if (!bit_is_set(PINA,PA5))
|
||
{
|
||
if(switchOneCheck)
|
||
{
|
||
lcdSetLayout(0);
|
||
lcdWriteStringP(menuTable[4]);
|
||
rfEnable = 1;
|
||
rfIdEnable();
|
||
roboMSPEnable();
|
||
switchOneCheck = 0;
|
||
}
|
||
}
|
||
|
||
// Active-mode 1 (left switch lower position)
|
||
if (!bit_is_set(PINA,PA6))
|
||
{
|
||
if(switchTwoCheck)
|
||
{
|
||
activateRobot(1);
|
||
switchTwoCheck = 0;
|
||
}
|
||
}
|
||
// Active-mode 2 (left switch middle position)
|
||
else if ((bit_is_set(PINA,PA7)) && (bit_is_set(PINA,PA6)))
|
||
{
|
||
if(!switchTwoCheck)
|
||
{
|
||
deactivateRobot();
|
||
rfEnable = 0;
|
||
switchTwoCheck = 1;
|
||
main();
|
||
}
|
||
}
|
||
// Active-mode 3 (left switch upper position)
|
||
else if (!bit_is_set(PINA,PA7))
|
||
{
|
||
if(switchTwoCheck)
|
||
{
|
||
activateRobot(2);
|
||
switchTwoCheck = 0;
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
|
||
return (0);
|
||
}
|
||
/*================================================================================================
|
||
End
|
||
================================================================================================*/
|
||
|