cm/robo-crawler
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
robo-crawler/RoboMI.c

1359 lines
30 KiB
C
Raw Permalink Blame History

/*================================================================================================
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
================================================================================================*/
Reference in New Issue View Git Blame Copy Permalink