maze_solver.txt

走迷宫机器人C++源码。使用IR Sensor

#include "system.h"
#include "stdio.h"
#include "altera_avalon_pio_regs.h"
#include "alt_types.h"
#include "sys/alt_irq.h"


///////////////////////////Input & Output Pins//////////////////////////

////////////////////IR Sensors Configuration////////////////////
//  Name            Pin Assignment      
//  
//  UpFront     =>  ATODINPUT0
//  UpRight     =>  ATODINPUT1
//  Upleft      =>  unused
//  DownFront   =>  ATODINPUT2
//  DownRight   =>  ATODINPUT3
//  Downleft    =>  unused
//  

////////////////////Motor PIN  Configuration////////////////////
//  Name                     Expression                       Pin Assignment    
//  LEFTENABLE               enable left motor to run              PIN_C1
//  LEFTDIRECTION            control left motor diection           PIN_A3
//  RIGHTENABLE              enable right motor to run             PIN_B4
//  RIGHTDIRECTION           control right motor direction         PIN_B5

//Example of PIN reading function using Altera: IORD_ALTERA_AVALON_PIO_DATA(UPSENSOR_BASE);
//Example of PIN writing function using Altera: IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x1);

//////////////////////// Global Variables ////////////////////////////////

int UpFront=0;    //Up sensor point out Front
int UpLeft=0;
int UpRight=0;   //Up sensor point out Left
int DownFront=0; //Down sensor point out front
int DownLeft=0;  //Down sensor point out Left
int DownRight=0;
int Balance=0;
int Balance_right=0;

const int seven = 1500000;        //This is a 7 degree turn aprox
const int eight = 3000000;  // This is 45 degree turn
const int quart = 22000000;  // This is 90 degree turn
const int bridge_right = 10000000; // bridge right turn 
const int half = 340;       // This is 180 degree turn
const int recover = 700000;


int ch0=0,ch1=0,ch2=0,ch3=0,ch4=0,ch5=0,ch6=0,ch7=0;
int sensor_cs=0;

void Forward();




/////////////////////////time Delay//////////////////////////
void wait ( int tick )
{
  int temp_tick;
  for(temp_tick=0;temp_tick<tick;temp_tick++);
}

/////////////////////////Stop moving Function//////////////////////////
void Stop()
    {
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x0);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x0);

    }// stop moving


/////////////////////////Read Sensor Function//////////////////////////

void Senser_reading()
{
int downfront_temp=0,downleft_temp=0,downright_temp=0;
int upfront_temp=0,upleft_temp=0,upright_temp=0;
int balance_temp=0,balance_right_temp=0;
int loop;
 
for(loop=0;loop<100;loop++)   
 { 
  if(IORD_ALTERA_AVALON_PIO_DATA(DOWNFRONT_BASE))
        downfront_temp++;
  if(IORD_ALTERA_AVALON_PIO_DATA(DOWNLEFT_BASE))
         downleft_temp++;
  if(IORD_ALTERA_AVALON_PIO_DATA(DOWNRIGHT_BASE))
        downright_temp++;
  if(IORD_ALTERA_AVALON_PIO_DATA(UPFRONT_BASE))
         upfront_temp++;
  if(IORD_ALTERA_AVALON_PIO_DATA(UPLEFT_BASE))
         upleft_temp++;
  if(IORD_ALTERA_AVALON_PIO_DATA(UPRIGHT_BASE))
         upright_temp++;
  if(IORD_ALTERA_AVALON_PIO_DATA(BALANCE_BASE))
         balance_temp++;
  if(IORD_ALTERA_AVALON_PIO_DATA(BALANCE_RIGHT_BASE))
         balance_right_temp++;

 }   

switch(downfront_temp)
 {
  case 100:
  DownFront=1;
  break;
  
  case 0:
  DownFront=0;
  break;
  
  default:
  DownFront=2;
 }

switch(downleft_temp)
 {
  case 100:
  DownLeft=1;
  break;
  
  case 0:
  DownLeft=0;
  break;
  
  default:
  DownLeft=2;
 }

switch(downright_temp)
 {
  case 100:
  DownRight=1;
  break;
  
  case 0:
  DownRight=0;
  break;
  
  default:
  DownRight=2;
 }

switch(upfront_temp)
 {
  case 100:
  UpFront=1;
  break;
  
  case 0:
  UpFront=0;
  break;
  
  default:
  UpFront=2;
 }

switch(upright_temp)
 {
  case 100:
  UpRight=1;
  break;
  
  case 0:
  UpRight=0;
  break;
  
  default:
  UpRight=2;
 }

switch(upleft_temp)
 {
  case 100:
  UpLeft=1;
  break;
  
  case 0:
  UpLeft=0;
  break;
  
  default:
  UpLeft=2;
 }

switch(balance_temp)
 {
  case 100:
  Balance=1;
  break;
  
  case 0:
  Balance=0;
  break;
  
  default:
  Balance=2;
 }

switch(balance_right_temp)
 {
  case 100:
  Balance_right=1;
  break;
  
  case 0:
  Balance_right=0;
  break;
  
  default:
  Balance_right=2;
 }
    }//End of Sensor reading fuction

//////////////////////////////////////// Motor Functions //////////////////////////////////

/////////////////////// Bridge Right turn ///////////////////////////////////

void Right_turn_bridge(int step)
    {
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTDIRECTION_BASE,0x1);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTDIRECTION_BASE,0x1);
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x3fff);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x2000);
    wait(step);
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x0);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x0);
    }// End of Right Turn


/////////////////////// Right turn stop///////////////////////////////////

void Left_turn_stop()
    {
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTDIRECTION_BASE,0x0);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTDIRECTION_BASE,0x0);
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x3fff);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x3fff);
    wait(6200000);
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x0);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x0);
    }// End of Right Turn


/////////////////////// Right turn balance///////////////////////////////////

void Right_turn_back(int step)
    {
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTDIRECTION_BASE,0x1);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTDIRECTION_BASE,0x1);
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x3fff);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x3000);
    wait(step);
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x0);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x0);
    }// End of Right Turn

/////////////////////// Right turn balance///////////////////////////////////

void Right_turn_balance(int step)
    {
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTDIRECTION_BASE,0x1);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTDIRECTION_BASE,0x0);
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x3fff);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x2bbb);
    wait(step);
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x0);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x0);
    }// End of Right Turn

////////////////////// Left Turn Balance///////////////////////////////////

void Left_turn_balance(int step)
    {
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTDIRECTION_BASE,0x1);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTDIRECTION_BASE,0x0);
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x2bbb);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x3fff);
    wait(step);
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x0);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x0);
    }// End of Left Turn

/////////////////////// Right turn///////////////////////////////////

void Right_turn()
    {
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTDIRECTION_BASE,0x1);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTDIRECTION_BASE,0x0);
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x3fee);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x2011);
    //wait(step);
   // IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x0);
   // IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x0);
    }// End of Right Turn

////////////////////// Left Turn///////////////////////////////////

void Left_turn()
    {
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTDIRECTION_BASE,0x1);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTDIRECTION_BASE,0x0);
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x0050);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x3fff);
    
    //wait(step);
   // IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x0);
    //IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x0);
    }// End of Left Turn





////////////////////// Forward ////////////////////////////////////

void Forward()
    {
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTDIRECTION_BASE,0x1);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTDIRECTION_BASE,0x0);
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x2eee);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x2eee);
    }// End of Forward

void UnbalancedForward()
    {
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTDIRECTION_BASE,0x1);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTDIRECTION_BASE,0x0);
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x2aaa);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x1666);
    }// End of Forward

void Forward_slow()
    {
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTDIRECTION_BASE,0x1);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTDIRECTION_BASE,0x0);
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x2555);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x2555);
    }// End of Forward

///////////////////// Turn Around ////////////////////////////////

void Turn_around()
    {
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTDIRECTION_BASE,0x1);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTDIRECTION_BASE,0x1);
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x3fff);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x3fff);
   // while(IORD_ALTERA_AVALON_PIO_DATA(UPFRONT_BASE));
    wait(12800000);
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x0);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x0);
        
    }// End of Turn Around
    
void Turn_left_around() // need test
    {
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTDIRECTION_BASE,0x0);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTDIRECTION_BASE,0x0);
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x3fff);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x2000);
   // while(IORD_ALTERA_AVALON_PIO_DATA(UPFRONT_BASE));
    wait(10000000);
    IOWR_ALTERA_AVALON_PIO_DATA(LEFTENABLE_BASE,0x0);
    IOWR_ALTERA_AVALON_PIO_DATA(RIGHTENABLE_BASE,0x0);
        
    }// End of Turn Around    
    
    
///////////////////// Balance function ////////////////////////////////

void Motion_balance()
{
     switch(Balance_right)
  {
  case 0:
   {IOWR_ALTERA_AVALON_PIO_DATA(LED_BASE,0x1);
   Right_turn_balance(seven);
   Forward();
   break;
   }
  case 1:
   {IOWR_ALTERA_AVALON_PIO_DATA(LED_BASE,0x0);
    Left_turn_balance(recover);
    Forward();
   break;
   }
  default:
  { Forward();
   IOWR_ALTERA_AVALON_PIO_DATA(LED_BASE,0x0);
  }
    } // end of switch

} // end of balance 
    

///////////////////// Motion Balance ////////////////////////////////


///////////////////// Hill & Bridge climbing  Function //////////////////////////
void BridgeHill() 
{ 
     UnbalancedForward();
     if(DownFront==0&&DownLeft==1)
         {Stop();
         wait(10000); 
         //Right_turn_bridge(bridge_right);
         Left_turn_stop();
         }
     if(DownFront==1&&DownRight==0)
        { Stop();
          wait(10000);
          Left_turn_balance(eight);
          UnbalancedForward();
        }
     if(DownFront==0&&DownLeft==0&&DownRight==0)
         {Stop();
         wait(10000);
         Turn_left_around();} // need test
 }// end of hill climb function

///////////////////// Maze  Function //////////////////////////
void Maze() 
{ 
/*
if(DownLeft==0&&UpFront==1)
{Right_turn_stop();
while(IORD_ALTERA_AVALON_PIO_DATA(DOWNFRONT_BASE)==1)
{
Forward_slow();
}
Stop();
Right_turn_stop();
}
*/
if(UpRight==0)
    { 
     while(IORD_ALTERA_AVALON_PIO_DATA(BALANCE_RIGHT_BASE)==1)
     Forward();
     //Stop();
    // wait(10000);
     
     while(IORD_ALTERA_AVALON_PIO_DATA(BALANCE_RIGHT_BASE)==0)   
     Right_turn();
     wait(11000);
    // Motion_balance();
    }
else
   {if(UpFront==0)
        Motion_balance();
    if(UpFront==1&&UpLeft==0)
     {  Forward();
        while(IORD_ALTERA_AVALON_PIO_DATA(BALANCE_BASE)==1);
        Forward();
        wait(6300000);
        Stop();
        Left_turn_stop(); // neeed test again
 //       while(IORD_ALTERA_AVALON_PIO_DATA(UPFRONT_BASE)==0)
        Forward();
        wait(8000000);
        Stop();  
      //  Right_turn_back(eight);
       // Stop();
       // wait(10000); 
       // while(IORD_ALTERA_AVALON_PIO_DATA(BALANCE_RIGHT_BASE)==0)
       // Right_turn();
      // Motion_balance();
     }
    if(UpFront==1&&UpLeft==1)
     {
     Forward();
     wait(10000000);
     Senser_reading();
     if(UpFront==1&&UpLeft==1)
     Turn_around();
     }
  
   }
    
}// end of Maze function


///////////////////////// System Health Function//////////////////////////////////////////////
void System_health()
{
    int health=5000000;
    IOWR_ALTERA_AVALON_PIO_DATA(LED_BASE,0x1);
    while(health--);
    IOWR_ALTERA_AVALON_PIO_DATA(LED_BASE,0x0);
}
///////////////////////// Main //////////////////////////////////////////////
// The main function has tree purposes:
// 1. All the control is maintained in the Main Function
// 2. This allows all the Turn and Sensor data to be called by
//    the rule so there is no error or contention.
// 3. The variables will be seperated and there will be no mix up
//    in the data that is collected or arranged in the main.
////////////////////////////////////////////////////////////////////////////

int main(void)
{
int main_delay=0,i;

System_health();// check whether system works

//Left_turn_stop();
//Left_turn_stop();
//Turn_around();
//Forward();
//for(i=0;i<500000000;i++);
//Left_turn(quart);
//Stop();

//Forward();
//for(i=0;i<500000000;i++);
//Left_turn(seven);
//for(i=0;i<500000000;i++);
//Right_turn(seven);
//Left_turn(recover);
//Forward();
//for(i=0;i<500000000;i++);
//Stop();

while(1)
{
Senser_reading();

//alt_printf("Upfront is %x \n",UpFront);
//alt_printf("UpLeft  is %x \n",UpLeft);

//i=IORD_ALTERA_AVALON_PIO_DATA(BALANCE_RIGHT_BASE);
//alt_printf("upleft    is %x \n",UpLeft);
//alt_printf("upfront   is %x \n",UpFront);
//alt_printf("upright   is %x \n",UpRight);
//alt_printf("downleft  is %x \n",DownLeft);
//alt_printf("downright is %x \n",DownRight);
//alt_printf("downfront is %x \n",DownFront);
//alt_printf("balance   is %x \n",Balance);
//alt_printf("Right balance   is %x \n",Balance_right);



//if(DownFront==1)
//Forward();
//for(i=0;i<500000000;i++);
//else
//Stop();


//alt_printf("DownFront is %x \n",DownFront);

//  UnbalancedForward();

if(UpFront==0&&UpLeft==0&&UpRight==0&&Balance==0&&Balance_right==0)
  BridgeHill(); 
else 
  Maze();

 
for(main_delay=0;main_delay<100000000;main_delay++) ;

}


return 0;
}// End of the Micromouse Program.