/*******************************************************************
* Function: int waitButton(void)
* Input Variables: none
* Output Return: int
* Overview: Use a comment block like this before functions
********************************************************************/
int WaitButton( void )
{
BOOL btnState1, btnState2, btnState3;//local variables - button states
//int rtnValue=0;//return the button value
int rtnValue=0;
LCD_clear();
if((ATopstat.state=SUBSYS_OPEN))
{
// Get switch states.
btnState1 = ATTINY_get_SW_state( ATTINY_SW3 );
btnState2 = ATTINY_get_SW_state( ATTINY_SW4 );
btnState3 = ATTINY_get_SW_state( ATTINY_SW5 );
//LCD_printf("btnStates: %d %d %d \n", btnState1, btnState2, btnState3);
if( btnState1 == TRUE )
{
LCD_printf( "SW1: Pushed\n");
// TMRSRVC_delay(500);//wait 1 s
// Assume the LED subsystem opened successfully.
LED_set_pattern( 0b00100000 );//turn the red LED on
TMRSRVC_delay(500);//wait 2 seconds
LED_clr_pattern( 0b01000000 );//turn the green LED off
LED_clr_pattern( 0b00100000 );//turn the red LED off
rtnValue=1;
}//end if button 1 state open
if( btnState2 == TRUE )
{
LCD_printf( "SW2: Pushed\n");
// TMRSRVC_delay(500);//wait 1 s
// Assume the LED subsystem opened successfully.
LED_set_pattern( 0b01000000 );//turn the green LED on
TMRSRVC_delay(500);//wait 2 seconds
LED_clr_pattern( 0b01000000 );//turn the green LED off
LED_clr_pattern( 0b00100000 );//turn the red LED off
rtnValue=2;
}//end if btn 2 open
if ( btnState3 == TRUE )
{
LCD_printf( "SW3: Pushed\n");
// TMRSRVC_delay(500);//wait 1 s
// Assume the LED subsystem opened successfully.
LED_set_pattern( 0b01000000 );//turn the green LED on
LED_set_pattern( 0b00100000 );//turn the red LED on
TMRSRVC_delay(500);//wait 2 seconds
LED_clr_pattern( 0b01000000 );//turn the green LED off
LED_clr_pattern( 0b00100000 );//turn the red LED off
rtnValue=3;
}//end if btn 3 open
LCD_clear();
return rtnValue;
}//end AT while
}//end the WaitButton() function
void displayCfgValues() {
// TMRO_TabEntry = nPSA+mSecScale*(PSA_256+1);
TMRO_TabEntry = nPSA+mSecScale*9;
LCD_printf(0,0, "PSA:%02X ", PSAValue[nPSA]);
LCD_print(0,9, mSecSCALE[mSecScale]);
LCD_printf(1,0, "TMR0Val:%04X", delayNum);
LCD_printf("%04X", TMR0_mSec[TMRO_TabEntry]);
}
// ============================================
// === LCD Display ===
// ----0----1----1----2
// ----5----0----5----0
// xx:TMR0 PSA:xx // extraCounter : tmr0Value : psaValue
// xx:xx 100mSec // delayVal : t0con : scale
// ============================================
void displayTMRValues() {
// LCD_Clear();
LCD_printf(0,0, "%02X:", pTMR->extraCounter);
LCD_printf( "%04X NO psa ", pTMR->tmr);
LCD_printf(1,0, "%02X", pTMR->delay);
LCD_printf( ":%02X", gcGateIndex);
LCD_printRom(1,9, gsTimeBase, pTMR->scaleOffs); // PRINT scale
}
// ============================================
// === LCD Display ===
// ----0----1----1----2
// ----5----0----5----0
// TMR0:xxxx PSA:xx
// Dly:xxx 100mS
// ============================================
void displayCfgValues() {
TMRO_TabEntry = stTMR0.psaValue + mSecScale*9;
LCD_printf(0,0, "TMR0:%04X", stTMR0.tmr0Value);
LCD_printf( " PSA:%02X", stTMR0.psaValue );
LCD_printf(1,0, "%02X", mSecScale);
LCD_printf(" %02X", stTMR0.extraCounter);
LCD_printf(" %02X", t0con);
LCD_print (1,11, stTMR0.scale);
}
// =======================================================================================
// ----5----0----5----0
// Frequency 100mS // Function : Scale
// Period 100mS // Function : Scale
// PosWave 100mS // +/- Pulse: numeroPulse (decimale 3digit): Scale
// NegWave 100mS // +/- Pulse: numeroPulse (decimale 3digit): Scale
// +Pulse:xxx 100mS // +/- Pulse: numeroPulse (decimale 3digit): Scale
// -Pulse:xxx 100mS // +/- Pulse: numeroPulse (decimale 3digit): Scale
// xxx.xxx.xxx MHz
// =======================================================================================
void displayValues() {
LCD_print (0,0, romFuncNames, PickRomString(romFuncNames, gcnFunc) ); // PRINT nome Funzione (Freq, Period, +Pulse ...
LCD_print (0,11, mSecScaleStr, pTMR->scaleOffs); // PRINT scale
LCD_printf(0,11, ":%02X", gcnFunc); //DEBUG
if ( (gcnFunc == nPULSE_POS) || (gcnFunc == nPULSE_NEG) ) {
if (fCONTINUOUS_KEY == true )
LCD_print (0,6, ": C "); // PRINT Continuos character
else
LCD_printf(0,6, ":%03d ", gcnPulse); // PRINT numero Pulse
}
}
/*******************************************************************
* Function: char moveTrackLight(void)
* Input Variables: void
* Output Return: char
* Overview: Tracks the light using only light lover behavior.
When it is right infront of the light, it outputs a unique
flag value that suppresses this behavior and inhibits
the moveRetreat behavior.
********************************************************************/
char moveTrackLight(void)
{
// make a variable that keeps track of the light tracking behavior
char isTracking = 0;
// check to see if there is too much light (is the robot in front of the light?)
if((leftLightVolt >= LIGHT_L_MAX)&&(rightLightVolt >= LIGHT_R_MAX))
{
// set global flags to 1
lightFlagStatus = 1;
retreatFlagStatus = 1;
// Act as a docking robot
LCD_printf("Arkin = Docked.\nPreparing to retreat.\n\n");
TMRSRVC_delay(3000);//wait 3 seconds
LCD_clear;
}
// else run moveLight(Lover) behavior
else
{
// inhibit LOVER behavior of move light
isTracking = moveLight(LIGHT_LOVER);
}
return isTracking;
}
/*
只重新绘制一个Button，这个Button必须是曾经绘制会过的
*/
BUTTON* BUTTON_redraw_button(BUTTON *pButton)
{
U16 fontWidth = FONT_get_string_width(pButton->content, FontFormat.fontType); //获得字符串的宽度
U16 fontHigh = FONT_get_lib_info(FontFormat.fontType)->fontHigh; //获得字体的高度
U32 oldColor; //旧颜色
U32 oldBackColor; //新颜色
U16 xStart;
U16 yStart;
LCD_draw_rectangle(pButton->xStart, pButton->yStart,
pButton->buttonWidth,
pButton->buttonHigh,
pButton->buttonColor);
oldColor = LCD_set_font_color(pButton->fontColor); //设置新颜色，保存旧颜色
oldBackColor = LCD_set_font_backColor(pButton->buttonColor); //设置新背景色,保存旧背景色
xStart = pButton->xStart + (pButton->buttonWidth - fontWidth)/2; //获得字体的x起始显示位置
yStart = pButton->yStart + (pButton->buttonHigh - fontHigh)/2; //获得字体的y起始显示位置
LCD_printf(xStart, yStart, pButton->content); //显示文字
LCD_set_font_backColor(oldBackColor); //恢复原来的字体背景色
LCD_set_font_color(oldColor); //恢复原来的字体颜色
return pButton; //返回这个按键
}
/*
绘制一个按钮的弹起效果
*/
BUTTON* BUTTON_draw_release_effect(BUTTON* pButton)
{
U16 fontWidth = FONT_get_string_width(pButton->content, FontFormat.fontType); //获得字符串的宽度
U16 fontHigh = FONT_get_lib_info(FontFormat.fontType)->fontHigh; //获得字体的高度
U32 oldColor; //旧颜色
U32 oldBackColor; //新颜色
U16 xStart;
U16 yStart;
pButton->buttonWidth = fontWidth > pButton->buttonWidth ? fontWidth : pButton->buttonWidth;
pButton->buttonHigh = fontHigh > pButton->buttonHigh ? fontHigh :pButton-> buttonHigh;
//如果指定的位置导致按钮会被显示在屏幕外面,则不显示，并返回0
if(pButton->xStart+pButton->buttonWidth > SCREEN_SIZE_X || pButton->yStart + pButton->buttonHigh > SCREEN_SIZE_Y)
return null;
LCD_draw_rectangle(pButton->xStart, pButton->yStart,
pButton->buttonWidth,
pButton->buttonHigh,
pButton->buttonColor);
oldColor = LCD_set_font_color(pButton->fontColor); //设置新颜色，保存旧颜色
oldBackColor = LCD_set_font_backColor(pButton->buttonColor); //设置新背景色,保存旧背景色
xStart = pButton->xStart + (pButton->buttonWidth - fontWidth)/2; //获得字体的x起始显示位置
yStart = pButton->yStart + (pButton->buttonHigh - fontHigh)/2; //获得字体的y起始显示位置
LCD_printf(xStart, yStart, pButton->content); //显示文字
LCD_set_font_backColor(oldBackColor); //恢复原来的字体背景色
LCD_set_font_color(oldColor); //恢复原来的字体颜色
return pButton;
}
void displayValues() {
//LCD_printRomOfs (0, 0, gsRomFuncNames, PickRomString(gsRomFuncNames, gcFuncIndex) ); // PRINT nome Funzione (Freq, Period, +Pulse ...
//LCD_printRomOfs (0,11, gsTimeBase, pTMR->scaleOffs); // PRINT GATE value
LCD_printRom(0, 0, gsRomFuncNames, PickRomString(gsRomFuncNames, gcFuncIndex) ); // PRINT nome Funzione (Freq, Period, +Pulse ...
LCD_printRom(0,11, gsTimeBase, pTMR->scaleOffs); // PRINT GATE value
// LCD_printf(1,11, ":%02X", gcFuncIndex); //DEBUG
// LCD_print (1, 0, "Ciao:"); //DEBUG
// LCD_gotoRC(1, 0);
// LCD_print(1,0, gsTimeBase, pTMR->extraCounter); // PRINT GATE value
//LCD_printfRom(1,0, ":%03d "); // PRINT numero Pulse
LCD_printfRom(1,0, ":%03d ", pTMR->scaleOffs); // PRINT numero Pulse
// LCD_printRom("CiaoRom"); // PRINT numero Pulse
// LCD_printRom(1,8, prova); // PRINT numero Pulse
LCD_printRom(1,11, "Rom"); // PRINT numero Pulse
// LCD_print (1, 0, gsRomFuncNames, PickRomString(gsRomFuncNames, gcFuncIndex) ); // PRINT nome Funzione (Freq, Period, +Pulse ...
if ( (gcFuncIndex == nPULSE_POS) || (gcFuncIndex == nPULSE_NEG) ) {
if (fCONTINUOUS_KEY == true )
LCD_print (0,6, ": C "); // PRINT Continuos character
else
LCD_printf(0,6, ":%03d ", gcPulseNumber); // PRINT numero Pulse
}
}
void updateLCD()
{
// Prints the robot position variables to the LCD screen
LCD_clear(); // Clears the LCD screen
LCD_printf(" %3.2f\n%3.2f %3.2f\n %3.2f\nX:%2.2f Y:%2.2f",ft,lt,rt,bk,roboxpos,roboypos);
}
/*******************************************************************
* Function: unsigned char shiftMap(unsigned char currentCell, unsigned char curMove, unsigned char curOrient)
* Input Variables: unsigned char, unsigned char, unsigned char
* Output Return: unsigned char
* Overview: shifts the map after robot moves
********************************************************************/
unsigned char shiftMap( unsigned char currentCell, unsigned char curMove, unsigned char curOrient)
{
// Get the currrent location of the robot
unsigned char curRow = currentCell >> 2;
unsigned char curCol = currentCell & 0b0011;
// // Git the currrent orientation of the robot
// unsigned char curOrient = currentOrientation;
switch(curMove){
case MOVE_LEFT:
//If we move left
// shift our oriention CCW
curOrient--;
curOrient = curOrient&0b11;
break;
case MOVE_FORWARD:
//If we move forward
// then shift to the next cell
// with repect to our curent oriention
switch(curOrient){
case NORTH:
curRow -= 1;
break;
case EAST:
curCol += 1;
break;
case SOUTH:
curRow += 1;
break;
case WEST:
curCol -= 1;
break;
default:
break;
}
break;
case MOVE_RIGHT:
//If we move right
// shift our oriention CW
curOrient++;
curOrient = curOrient&0b11;
break;
default:
LCD_printf("Whatz3?!");
break;
}
// Set the new cell of the robot
currentCell = (curRow << 2) + curCol;
// Set the new orientation of the robot
currentOrientation = curOrient;
return currentCell;
}
/*
* a function that can test that sensor data can be output
* to the LCD display in a meaningful manner and the normal
* printf still works correctly
*/
void pixel_sensor_test(void) {
while(checkNoBtns()) {
TMRSRVC_delay(100);
LCD_clear();
float leftIR = getLeftIR();
float rightIR = getRightIR();
float trimLeftIR = trim(2*leftIR, 0, LCD_PIX_WIDTH/2);
float trimRightIR = trim(2*rightIR, 0, LCD_PIX_WIDTH/2);
for(unsigned char i = 0; i < LCD_PIX_HEIGHT; i++) {
LCD_set_pixel(i, LCD_PIX_WIDTH/2, i & 1);
LCD_set_pixel(i, LCD_PIX_WIDTH/2 - trimLeftIR, 1);
LCD_set_pixel(i, LCD_PIX_WIDTH/2-1 + trimRightIR, 1);
}
LCD_set_RC( 0, 0 );
LCD_printf("%.1f", (double)leftIR);
LCD_set_RC( 0, 16 );
LCD_printf("%.1f", (double)rightIR);
}
}
void wander(char type)
{
if(type == 'a')
{
while(1){
char robostate='w';
updateLCD();
while (robostate=='w')
{
robostate = aggressiveKid(1,6,3);
getIR();
updateLCD();
TMRSRVC_delay(100);
}
LCD_clear();
LCD_printf("Reason for Stop: %c",robostate);
if(ATTINY_get_SW_state(ATTINY_SW3))
break;
}
}
if(type=='s')
{
while(1){
char robostate='w';
updateLCD();
while (robostate=='w')
{
robostate = shyGuy(1,7,4,4,6);
getIR();
updateLCD();
TMRSRVC_delay(100);
}
LCD_clear();
LCD_printf("Reason for Stop: %c",robostate);
if(ATTINY_get_SW_state(ATTINY_SW3))
break;
}
}
}
/*******************************************************************
* Function: void planGateway(void)
* Input Variables: void
* Output Return: void
* Overview: build the localizeGateways tree with current branch
********************************************************************/
void planGateway( void )
{
// If we are still lost
// when we reach our max branch
// Pop of the seed and use the second oldest
// as the new seed
unsigned char i;
if(currentBranch >= BRANCH_MAX){
for(i = 0; i < BRANCH_MAX; i++){
localizeGateways[0][i] = localizeGateways[0][1+i];
localizeGateways[1][i] = localizeGateways[1][1+i];
localizeGateways[2][i] = localizeGateways[2][1+i];
}
currentBranch = BRANCH_MAX-1;
}
// Decide what the current move should be
planMap();
// Save the current gateway, move, and orientation
localizeGateways[0][currentBranch] = currentGateway;
localizeGateways[1][currentBranch] = currentMove;
localizeGateways[2][currentBranch] = currentOrientation;
// Update the currentOrientation using currentMove
switch(currentMove){
case MOVE_LEFT:
// If we move left
// shift our oriention CCW
currentOrientation--;
currentOrientation = currentOrientation&0b11;
break;
case MOVE_FORWARD:
break;
case MOVE_RIGHT:
// If we move right
// shift our oriention CW
currentOrientation++;
currentOrientation = currentOrientation&0b11;
break;
default:
LCD_printf("Whatz2?!");
break;
}
// If we have none or more than one seed
// Increment current branch
currentBranch++;
}
void test(void) {
// Init button
P1SEL &= ~BIT7;
P1DIR &= ~BIT7;
P1REN |= BIT7;
P1OUT |= BIT7;
//Set 1:
// Test \n and \r behaviour
LCD_printf("Test1\r\n");
LCD_printf("Test2\n");
LCD_printf("Test3\n");
wait_for_button_and_clear();
//Set 2 :
// Print N numbers with a newline
for(uint8_t i = 0; i < 20; i++) {
LCD_printf("%02u\r\n", i);
}
wait_for_button_and_clear();
// Set 3:
// Test tabs
LCD_printf("Tab test:\r\n");
LCD_printf("\tTabbed line\r\n");
wait_for_button_and_clear();
// Set 4:
// Test \r overwrite
LCD_printf("Test1\r");
LCD_printf("Test2");
// Set 5:
// Print all chars
for(uint8_t i = ' '; i < (' ' + 105); i++) {
LCD_printf("%c", i);
}
while(1);
}
void main() {
Init_Registers();
initPorts();
LCD_initialize();
LCD_Clear();
while (1) {
TMR0_GATE_LINE = 0; //Turn OFF Gate line
LCD_print(0,0, "Line1"); // LCD_Line1-0
LCD_printf(0,7, ":%03d ", 54); // LCD_Line1-7
LCD_printRom(1,0, "Line2"); // LCD_Line2-0
delay_s(1);
TMR0_GATE_LINE = 1; //Turn OFF Gate line
delay_s(1);
}
}
/*
按照pButton描述的方式显示一个button
在函数执行过程中，带入的按键的宽和高可能会被改变
当文字要求的宽度或高度大于按钮提供的宽度或高度时，按钮的宽度或高度会增加到文字的宽度或高度
在函数执行过程中，按键的xStart和yStart属性会被初始化
如果函数成功的将按钮绘制到屏幕上，则同时将该按钮添加到按钮链表中
函数可能会绘制按键失败: 按键的要求宽度或高度与其显示位置的综合使按钮显示在了屏幕之外.
*/
BUTTON* BUTTON_draw_button(U32 xStart, U32 yStart, BUTTON *pButton)
{
U16 fontWidth = FONT_get_string_width(pButton->content, FontFormat.fontType); //获得字符串的宽度
U16 fontHigh = FONT_get_lib_info(FontFormat.fontType)->fontHigh; //获得字体的高度
U32 oldColor; //旧颜色
U32 oldBackColor; //新颜色
pButton->xStart = xStart;
pButton->yStart = yStart;
//pButton->buttonWidth = fontWidth > pButton->buttonWidth ? fontWidth + 10: pButton->buttonWidth;
//pButton->buttonHigh = fontHigh > pButton->buttonHigh ? fontHigh + 10:pButton-> buttonHigh;
//如果指定的位置导致按钮会被显示在屏幕外面,则不显示，并返回0
if(pButton->xStart+pButton->buttonWidth > SCREEN_SIZE_X || pButton->yStart + pButton->buttonHigh > SCREEN_SIZE_Y)
return null;
LCD_draw_rectangle(xStart, yStart,
pButton->buttonWidth,
pButton->buttonHigh,
pButton->buttonColor);
oldColor = LCD_set_font_color(pButton->fontColor); //设置新颜色，保存旧颜色
oldBackColor = LCD_set_font_backColor(pButton->buttonColor); //设置新背景色,保存旧背景色
xStart = xStart + (pButton->buttonWidth - fontWidth)/2; //获得字体的x起始显示位置
yStart = yStart + (pButton->buttonHigh - fontHigh)/2; //获得字体的y起始显示位置
LCD_printf(xStart, yStart, pButton->content); //显示文字
LCD_set_font_backColor(oldBackColor); //恢复原来的字体背景色
LCD_set_font_color(oldColor); //恢复原来的字体颜色
//如果显示成功,则将该按键加入到按键链表中
if(PButtonList == null)
BUTTON_init();
BTNLIST_insert_button(pButton, PButtonList);
return pButton; //返回这个按键
}
void levelZero(char type,float frontDist)
{
if(type == 'a')
{
while(1){
char robostate='w';
updateLCD();
while (robostate=='w')
{
robostate = aggressiveKid(0,6,3);
getIR();
updateLCD();
TMRSRVC_delay(100);
}
LCD_clear();
LCD_printf("Reason for Stop: %c",robostate);
if(ATTINY_get_SW_state(ATTINY_SW3))
break;
}
}
}
/*******************************************************************
* Function: void map (void)
* Input Variables: none
* Output Return: none
* Overview: Makes the robot map the world
********************************************************************/
void map (void)
{
// Initialize State
isMapping = 1;
// Mapping Loop
while(isMapping)
{
//Sense
checkIR();
checkWorld();
//Record
setGateways();
//Plan using the Map
planMap();
//Act on the Map
moveMap();
//Shift the Map
currentCellWorld = shiftMap(currentCellWorld, currentMove, currentOrientation);
//Break?
isMapping = !((currentCellWorldStart == currentCellWorld)&&(currentOrientationStart == currentOrientation));
if(!isMapping){
break;
}
//Print Map
LCD_clear();
LCD_printf(" Move"BYTETOBINARYPATTERN"\n Cell"BYTETOBINARYPATTERN"\n Ornt"BYTETOBINARYPATTERN"\n\n",BYTETOBINARY(currentMove),BYTETOBINARY(currentCellWorld),BYTETOBINARY(currentOrientation));
printMap(currentOrientation,currentCellWorld,RESET);
TMRSRVC_delay(500);//wait 3 seconds
}
}
/*******************************************************************
* Function: void moveMap(void)
* Input Variables: void
* Output Return: void
* Overview: moves the robot through the map
********************************************************************/
void moveMap( void )
{
switch(currentMove){
case MOVE_LEFT:
move_arc_stwt(POINT_TURN, LEFT_TURN, 10, 10, 0);
break;
case MOVE_FORWARD:
// checkOdometry(1);
// while(!odometryFlag){
// moveWall();
// checkOdometry(0);
// }
move_arc_stwt(NO_TURN, 45, 10, 10, 0);
break;
case MOVE_RIGHT:
move_arc_stwt(POINT_TURN, RIGHT_TURN, 10, 10, 0);
break;
default:
LCD_printf("Whatz2?!");
break;
}
}
/*******************************************************************
* Function: void moveMap(void)
* Input Variables: void
* Output Return: void
* Overview: moves the robot through the map
********************************************************************/
void moveMap( void )
{
char isDone = 0;
pidController(0,RESET);
switch(currentMove){
case MOVE_LEFT:
move_arc_stwt(POINT_TURN, LEFT_TURN, 30, 30, 0);
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_ON);
TMRSRVC_delay(BRAKE_DELAY);
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_OFF);
break;
case MOVE_FORWARD:
setOdometry(WALL_STEP);
while(!isDone){
checkIR();
isDone = moveWall();
}
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_ON);
TMRSRVC_delay(BRAKE_DELAY);
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_OFF);
// move_arc_stwt(NO_TURN, 45, 10, 10, 0);
break;
case MOVE_RIGHT:
move_arc_stwt(POINT_TURN, RIGHT_TURN, 30, 30, 0);
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_ON);
TMRSRVC_delay(BRAKE_DELAY);
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_OFF);
break;
default:
LCD_printf("Whatz2?!");
break;
}
}
/*******************************************************************
* Function: void planMap(void)
* Input Variables: void
* Output Return: void
* Overview: maps the world as it moves through it
********************************************************************/
void planMap( void )
{
//Plan
if(!(currentGateway&0b0001)){
// If we can make a left turn,
// then spin left
currentMove = MOVE_LEFT;
}
else if(!(currentGateway&0b1000)){
// If we can't make a left turn,
// but we can go forward,
// go forward
currentMove = MOVE_FORWARD;
}
else {
// If we can't turn left or go forward
// then spin right
currentMove = MOVE_RIGHT;
}
switch(oldMove){
case MOVE_LEFT:
//If we turned left befor
//then we still have yet to go forward
currentMove = MOVE_FORWARD;
break;
case MOVE_FORWARD:
break;
case MOVE_RIGHT:
break;
default:
LCD_printf("Whatz1?!");
break;
}
oldMove = currentMove;
}
/*******************************************************************
* Function: void metricMove(void)
* Input Variables: none
* Output Return: none
* Overview: Moves around the map using metric navigation
********************************************************************/
void metricMove (void)
{
switch(currentOrientation){
case NORTH:
switch(nextOrientation){
case NORTH:
currentMove = MOVE_FORWARD; break;
case EAST:
currentMove = MOVE_RIGHT; break;
case SOUTH:
currentMove = MOVE_LEFT; break;
case WEST:
currentMove = MOVE_LEFT; break;
default:
LCD_printf("Whatz5?!"); break;
}
break;
case EAST:
switch(nextOrientation){
case NORTH:
currentMove = MOVE_LEFT; break;
case EAST:
currentMove = MOVE_FORWARD; break;
case SOUTH:
currentMove = MOVE_RIGHT; break;
case WEST:
currentMove = MOVE_LEFT; break;
default:
LCD_printf("Whatz5?!"); break;
}
break;
case SOUTH:
switch(nextOrientation){
case NORTH:
currentMove = MOVE_LEFT; break;
case EAST:
currentMove = MOVE_LEFT; break;
case SOUTH:
currentMove = MOVE_FORWARD; break;
case WEST:
currentMove = MOVE_RIGHT; break;
default:
LCD_printf("Whatz5?!"); break;
}
break;
case WEST:
switch(nextOrientation){
case NORTH:
currentMove = MOVE_RIGHT; break;
case EAST:
currentMove = MOVE_LEFT; break;
case SOUTH:
currentMove = MOVE_LEFT; break;
case WEST:
currentMove = MOVE_FORWARD; break;
default:
LCD_printf("Whatz5?!"); break;
}
break;
default:
LCD_printf("Whatz5?!"); break;
}
}
/*******************************************************************
* Function: void metric(void)
* Input Variables: none
* Output Return: none
* Overview: Moves the robot to the goal
********************************************************************/
void metric (void)
{
// currentCellWorld = 0b0000;
// currentGoalWorld = 15;
// Make metric map
wavefrontMake();
// Initialize State
isGoal = 0;
unsigned char isSiren = 0;
// Metric Loop
while(!isGoal){
if(isSiren){
SPKR_beep(500);
isSiren = 0;
}
else{
SPKR_beep(250);
isSiren = 1;
}
LCD_clear();
switch(currentOrientation){
case NORTH:
LCD_printf("CurtOrent:NORTH\n");
break;
case EAST:
LCD_printf("CurtOrent:EAST\n");
break;
case SOUTH:
LCD_printf("CurtOrent:SOUTH\n");
break;
case WEST:
LCD_printf("CurtOrent:WEST\n");
break;
default:
break;
}
// Find the next orentation
isGoal = fourNeighborSearch(currentCellWorld);
if(isGoal){
break;
}
switch(nextOrientation){
case NORTH:
LCD_printf("NextOrent:NORTH\n");
break;
case EAST:
LCD_printf("NextOrent:EAST\n");
break;
case SOUTH:
LCD_printf("NextOrent:SOUTH\n");
break;
case WEST:
LCD_printf("NextOrent:WEST\n");
break;
default:
break;
}
switch(currentMove){
case MOVE_LEFT:
LCD_printf("CurMOVE:LEFT\n");
break;
case MOVE_RIGHT:
LCD_printf("CurMOVE:RIGHT\n");
break;
case MOVE_FORWARD:
LCD_printf("CurMOVE:FORWARD\n");
break;
default:
break;
}
// Plan using metric map and next orientation
planMetric();
// Act on the move
moveMap();
// Shift the map
currentCellWorld = shiftMap(currentCellWorld, currentMove, currentOrientation);
// TMRSRVC_delay(2000);//wait 1 seconds
}
SPKR_beep(0);
}
void CBOT_main( void )
{
// Initialize Robot
ATopstat = ATTINY_open();//open the tiny microcontroller
LEopstat = LED_open(); //open the LED module
LCopstat = LCD_open(); //open the LCD module
STEPPER_open(); // Open STEPPER module for use
SPKR_open(SPKR_BEEP_MODE);//open the speaker in beep mode
LED_open();
I2C_open();
ADC_open();//open the ADC module
ADC_set_VREF( ADC_VREF_AVCC );// Set the Voltage Reference first so VREF=5V.
// // Print a debug statement
// LCD_printf("It's ALIVE\n\n\n\n");
// TMRSRVC_delay(3000);//wait 3 seconds
// LCD_clear();
// Print the metric Map
// printMetricMap();
// LCD_printf("%i %i %i %i \n%i %i %i %i \n%i %i %i %i \n%i %i %i %i \n",ROBOT_METRIC_WORLD[0][0],ROBOT_METRIC_WORLD[0][1],ROBOT_METRIC_WORLD[0][2],ROBOT_METRIC_WORLD[0][3],ROBOT_METRIC_WORLD[1][0],ROBOT_METRIC_WORLD[1][1],ROBOT_METRIC_WORLD[1][2],ROBOT_METRIC_WORLD[1][3],ROBOT_METRIC_WORLD[2][0],ROBOT_METRIC_WORLD[2][1],ROBOT_METRIC_WORLD[2][2],ROBOT_METRIC_WORLD[2][3],ROBOT_METRIC_WORLD[3][0],ROBOT_METRIC_WORLD[3][1],ROBOT_METRIC_WORLD[3][2],ROBOT_METRIC_WORLD[3][3]);
// TMRSRVC_delay(10000);//wait 10 seconds
// wavefrontMake();
// LCD_printf("%i %i %i %i \n%i %i %i %i \n%i %i %i %i \n%i %i %i %i \n",ROBOT_METRIC_WORLD[0][0],ROBOT_METRIC_WORLD[0][1],ROBOT_METRIC_WORLD[0][2],ROBOT_METRIC_WORLD[0][3],ROBOT_METRIC_WORLD[1][0],ROBOT_METRIC_WORLD[1][1],ROBOT_METRIC_WORLD[1][2],ROBOT_METRIC_WORLD[1][3],ROBOT_METRIC_WORLD[2][0],ROBOT_METRIC_WORLD[2][1],ROBOT_METRIC_WORLD[2][2],ROBOT_METRIC_WORLD[2][3],ROBOT_METRIC_WORLD[3][0],ROBOT_METRIC_WORLD[3][1],ROBOT_METRIC_WORLD[3][2],ROBOT_METRIC_WORLD[3][3]);
// TMRSRVC_delay(10000);//wait 10 seconds
// LCD_clear();
// Test the 4-Neighbor search
// LCD_printf("%i %i %i %i \n%i %i %i %i \n%i %i %i %i \n%i %i %i %i \n",ROBOT_METRIC_WORLD[0][0],ROBOT_METRIC_WORLD[0][1],ROBOT_METRIC_WORLD[0][2],ROBOT_METRIC_WORLD[0][3],ROBOT_METRIC_WORLD[1][0],ROBOT_METRIC_WORLD[1][1],ROBOT_METRIC_WORLD[1][2],ROBOT_METRIC_WORLD[1][3],ROBOT_METRIC_WORLD[2][0],ROBOT_METRIC_WORLD[2][1],ROBOT_METRIC_WORLD[2][2],ROBOT_METRIC_WORLD[2][3],ROBOT_METRIC_WORLD[3][0],ROBOT_METRIC_WORLD[3][1],ROBOT_METRIC_WORLD[3][2],ROBOT_METRIC_WORLD[3][3]);
// TMRSRVC_delay(5000);
// LCD_clear();
currentCellWorld = WORLD_CELL[0][0];
wavefrontMake();
while(currentCellWorld!=reachedEnd){
nextOrientation = fourNeighborSearch(currentCellWorld);
metricMove();
moveMap();
currentCellWorld = shiftMap(currentCellWorld, currentMove, currentOrientation);
}
LCD_printf("LOOOOOOOOOOOOLZ");
TMRSRVC_delay(5000);
LCD_clear();
// Unit test the function
// sqrt function
// int xDelta, yDelta;
// float distance;
// xDelta = abs(-3);
// yDelta = abs(0);
// distance = sqrt(((xDelta*xDelta)+(yDelta*yDelta)));
// LCD_printf("xDel = %d\nyDel = %d\ndist = %f\n\n",xDelta,yDelta,distance);
// TMRSRVC_delay(5000);
// LCD_clear();
// cell values
}// end the CBOT_main()
void main(void)
{
#if 0
// Config data in config.hex, implemented by app_config.c code
Set_Pin_Drive_Mode(REG_PRT2_PC0, PRT_PC__DRIVE_MODE__STRONG);
Set_Pin_Drive_Mode(REG_PRT2_PC1, PRT_PC__DRIVE_MODE__STRONG);
Set_Pin_Drive_Mode(REG_PRT2_PC2, PRT_PC__DRIVE_MODE__STRONG);
Set_Pin_Drive_Mode(REG_PRT2_PC3, PRT_PC__DRIVE_MODE__STRONG);
Set_Pin_Drive_Mode(REG_PRT2_PC4, PRT_PC__DRIVE_MODE__STRONG);
Set_Pin_Drive_Mode(REG_PRT2_PC5, PRT_PC__DRIVE_MODE__STRONG);
Set_Pin_Drive_Mode(REG_PRT2_PC6, PRT_PC__DRIVE_MODE__STRONG);
#endif
LCD_init_4bit(2, LCD_FONT_5x10);
LCD_moveto(0,0);
LCD_write_string("Hello PSoC 2c");
LCD_moveto(0,1);
LCD_write_data('X');
LCD_moveto(3,1);
LCD_write_data('Y');
LCD_moveto(11,1);
LCD_printf("ver %d", 1);
#if 0
// NOTE: The following pin() functions have been moved to config.hex
// LED: OUT: Port 6.0 Strong, initially OFF
// DM 2,1,0: Strong: 1 1 0 (DATA = 1 ON, 0 OFF)
Set_Pin_Drive_Mode(REG_PRT6_PC0, PRT_PC__DRIVE_MODE__STRONG);
Clear_Pin(REG_PRT6_PC0);
// LED: OUT: Port 6.6 Strong, initially OFF
// DM 2,1,0: Strong: 1 1 0 (DATA = 1 ON, 0 OFF)
Set_Pin_Drive_Mode(REG_PRT6_PC6, PRT_PC__DRIVE_MODE__STRONG);
Clear_Pin(REG_PRT6_PC6);
// Button: IN: Port 6.1 Pull Up
// DM 2,1,0: Pullup: 0 1 0 (Data = 1 pullup 5K Vcc) [not 0 - hard 0V]
Set_Pin_Drive_Mode(REG_PRT6_PC1, PRT_PC__DRIVE_MODE__RES_PULLUP);
Set_Pin(REG_PRT6_PC1);
#endif
bool led2 = false;
uint32_t count = 0;
// Note: This is a busy wait loop - ok for a demo but not for product code.
while(1)
{
if (Read_Pin_Raw(REG_PRT6_PC1))
Clear_Pin(REG_PRT6_PC0);
else
Set_Pin(REG_PRT6_PC0);
delay_ms(10);
if (count++ % 100 == 0) // 10 * 100 = 1000 ms
led2 = !led2;
if (led2)
Clear_Pin(REG_PRT6_PC6);
else
Set_Pin(REG_PRT6_PC6);
};
// don't return
}
void LCD_printf(unsigned char LcdRow, unsigned char LcdColumn, const unsigned char *lcdString, unsigned int val ) {
LCD_gotoRC(LcdRow, LcdColumn);
LCD_printf(lcdString, val );
}
/*
* Prints the main menu
*/
void printMainMenu(void) {
LCD_clear();
LCD_printf("Btn 1: All Pixels\nBtn 2: Pixel Sensor\nBtn 3: Smiley\n");
}
int16_t LCD_printLine(const char * fmt)
{
LCD_printf("\n");
return LCD_printf(fmt);
}
void CBOT_main( void )
{
// initialize the robot
initializeRobot();
currentOrientation = NORTH;
// Ask for Goal
char isDone = 0;
unsigned char btnHolder = UNPRESSED;
LCD_clear();
LCD_printf(" Goal?\n\n\n\n");
while(!isDone){
btnHolder = EnterTopoCommand();
switch(btnHolder){
case MOVE_LEFT:
currentGoalWorld--;
currentGoalWorld = currentGoalWorld&0b1111;
break;
case MOVE_FORWARD:
isDone = 1;
break;
case MOVE_RIGHT:
currentGoalWorld++;
currentGoalWorld = currentGoalWorld&0b1111;
break;
default:
break;
}
printMap(currentOrientation,currentGoalWorld,RESET);
TMRSRVC_delay(100);//wait .1 seconds
}
// Ask for starting orentation
isDone = 0;
btnHolder = UNPRESSED;
LCD_clear();
LCD_printf(" Orient?\n\n\n\n");
while(!isDone){
btnHolder = EnterTopoCommand();
switch(btnHolder){
case MOVE_LEFT:
// If we move left
// shift our oriention CCW
currentOrientation--;
currentOrientation = currentOrientation&0b11;
break;
case MOVE_FORWARD:
isDone = 1;
break;
case MOVE_RIGHT:
// If we move right
// shift our oriention CW
currentOrientation++;
currentOrientation = currentOrientation&0b11;
break;
default:
break;
}
printMap(currentOrientation,currentGoalWorld,RESET);
TMRSRVC_delay(100);//wait .1 seconds
}
// Ask to start
isDone = 0;
btnHolder = UNPRESSED;
LCD_clear();
LCD_printf(" Start?\n\n\n\n");
while(!isDone){
btnHolder = EnterTopoCommand();
switch(btnHolder){
case MOVE_LEFT:
break;
case MOVE_FORWARD:
isDone = 1;
break;
case MOVE_RIGHT:
break;
default:
break;
}
printMap(currentOrientation,currentGoalWorld,RESET);
TMRSRVC_delay(100);//wait .1 seconds
}
// Locilize the Robot
// localize();
// Initialize State
isLost = 1;
oldMove = MOVE_STOP;
// Localization Loop
while(isLost)
{
// Break if not isLost
if(!isLost){
break;
}
//Sense Gateway
checkIR();
checkWorld();
//Plan using the Gateway
planGateway();
//Localize from Gateways?
isLost = localizeGateway();
//Act on the Gateway
moveMap();
}
// Update the currentOrientation using currentMove
switch(currentMove){
// case MOVE_LEFT:
// // If we move left
// // shift our oriention CCW
// currentOrientation--;
// currentOrientation = currentOrientation&0b11;
// break;
case MOVE_FORWARD:
break;
// case MOVE_RIGHT:
// // If we move right
// // shift our oriention CW
// currentOrientation++;
// currentOrientation = currentOrientation&0b11;
// break;
default:
LCD_printf("Whatz2?!");
break;
}
SPKR_beep(500);
// LCD_clear();
// LCD_printf("LOLZ\nI'm found!");
// TMRSRVC_delay(3000);//wait 3 seconds
LCD_clear();
LCD_printf(" New Map\n\n\n\n");
printMap(currentOrientation,currentCellWorld,RESET);
TMRSRVC_delay(1000);//wait 1 seconds
SPKR_beep(0);
// currentCellWorld = 0;
isFire = 0;
// Go firefight
while(!isFire){
//Sense Gateway
checkIR();
checkWorld();
isFire = checkFire();
if(isFire){
break;
}
// Plan using Map
planMap();
// Shift the map
currentCellWorld = shiftMap(currentCellWorld, currentMove, currentOrientation);
// Act on the Map
moveMap();
}
// // Beep for the fire SIREN
// int ii;
// for (ii=0; ii<=3; ii++){
// SPKR_beep(250);
// TMRSRVC_delay(1000);
// SPKR_beep(500);
// TMRSRVC_delay(1000);
// }
// SPKR_beep(0);
// // Print the fire cell location
// LCD_clear();
// LCD_printf("Fire = %i\n\n\n\n", currentFireCell);
// TMRSRVC_delay(5000);
// Moves the Robot to the goal
metric();
// Print that you are at home and the fire cell location
LCD_clear();
LCD_printf("LOLZ\nI'm HOME\nFire at Cell: %i\n\n",currentFireCell);
// Stop when home is reached
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_OFF);
// Beep when home is reached
SPKR_beep(500);
TMRSRVC_delay(3000);//wait 3 seconds
SPKR_beep(0);
TMRSRVC_delay(7000);//wait 7 seconds
/**
// Enter the robot's current (starting) position
LCD_printf("START Map/nlocation\n\n\n");
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
worldInput();
TMRSRVC_delay(1000);//wait 3 seconds
LCD_clear();
// Enter the robot's current (starting) orientation
LCD_printf("START Map/norientation\n\n\n");
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
orientationInput();
TMRSRVC_delay(1000);//wait 3 seconds
LCD_clear();
// Print the map
LCD_clear();
printMap(currentOrientation,currentCellWorld,RESET);
TMRSRVC_delay(10000);//wait 10 seconds
LCD_clear();
// Enter the robot's current (starting) position
LCD_printf("START Path\nlocation\n\n\n");
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
worldInput();
TMRSRVC_delay(1000);//wait 3 seconds
LCD_clear();
// Enter the robot's current (starting) orientation
LCD_printf("START Path\norientation\n\n\n");
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
orientationInput();
TMRSRVC_delay(1000);//wait 3 seconds
LCD_clear();
// Enter the robot topological commands
LCD_printf("ENTER Path\ncommands\n\n\n");
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
movesInput();
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
// Print the robot gateways
LCD_printf("Robot Gateways:\n\n\n\n");
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
getGateways();
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
// Goal loop
while (1)
{
checkIR();
checkWorld();
moveWorld();
//Test arc function
// LCD_printf("Move Arc\n\n\n\n");
// TMRSRVC_delay(1000);//wait 1 seconds
// move_arc_stwt(POINT_TURN, LEFT_TURN, 10, 10, 0);
// //Test contact Sensors
// LCD_printf("Right Contact: %i\nLeft Contact: %i\n\n\n",rightContact,leftContact);
// TMRSRVC_delay(1000);//wait 1 seconds
// // Test IR Sensors
// LCD_clear();
// LCD_printf("FrontIR = %3.2f\nBackIR = %3.2f\nLeftIR = %3.2f\nRightIR = %3.2f\n", ftIR,bkIR,ltIR,rtIR);
// TMRSRVC_delay(1000);//wait 1 seconds
}
**/
}// end the CBOT_main()
