motors.c

控制pic系统小车

/****************************************************************
* 			Control proximity sensor of e-puck	with timer 1	*	 		*
*			December 2004: first version                        *
*			Lucas Meier & Francesco Mondada  					*
*			Version 1.0 november 2005							*
*			Michael Bonani										*
*																*
****************************************************************/


#include "ad_conv.h"
#include <stdlib.h>
#include "epuck_ports.h"
#include "init_port.h"
#include "prox.h"
/* internal variables for prox */
int ambient_and_reflected_ir[8]; // light when led is on二极管是否发光
int ambient_ir[8]; // light when led is on二极管是否发光
int reflected_ir[8];
//motor	
int left_speed = 0;
int right_speed = 0;
int	nbr_pas_left = 0;
int nbr_pas_right = 0;	
void InitTMR5(void)
{
  T5CON = 0;                    // 
  T5CONbits.TCKPS=3;            // prescsaler = 256
  TMR5 = 0;                     // clear timer 5
  PR5 = (500.0*MILLISEC)/left_speed ; // interrupt every 500ms with 256 prescaler
  IFS1bits.T5IF = 0;            // clear interrupt flag
  IEC1bits.T5IE = 1;            // set interrupt enable bit
  T5CONbits.TON = 1;            // start Timer5
 
}
void InitTMR4(void)
{
  T4CON = 0;                    // 
  T4CONbits.TCKPS=3;            // prescsaler = 256
  TMR4 = 0;                     // clear timer 4
  PR4 = (500.0*MILLISEC)/right_speed; // interrupt every 500ms with 256 prescaler
  IFS1bits.T4IF = 0;            // clear interrupt flag
  IEC1bits.T4IE = 1;            // set interrupt enable bit
  T4CONbits.TON = 1;            // start Timer5
 
}
void _ISRFAST _T5Interrupt(void) // interrupt for motor 1 (of two) = left motor

{
  static int motor_phase=0;		 // phase can be 0 to 3

  IFS1bits.T5IF = 0;             // clear interrupt flag

  // increment or decrement phase depending on direction

  if (left_speed> 0) // inverted for the two motors
    {
    nbr_pas_left++;
	motor_phase--;
    if (motor_phase < 0) motor_phase = 3;
    }
  else 
    {
    nbr_pas_left--;
    motor_phase++;
    if (motor_phase > 3) motor_phase = 0;
    }
  
  // set the phase on the port pins

  switch (motor_phase)
  {
    case 0:
    {
      motor1_pha = 0;
      motor1_phb = 1;
      motor1_phc = 0;
      motor1_phd = 1;
      break;
    }
    case 1:
    {
      motor1_pha = 0;
      motor1_phb = 1;
      motor1_phc = 1;
      motor1_phd = 0;
      break;
    }
    case 2:
    {
      motor1_pha = 1;
      motor1_phb = 0;
      motor1_phc = 1;
      motor1_phd = 0;
      break;
    }
    case 3:
    {
      motor1_pha = 1;
      motor1_phb = 0;
      motor1_phc = 0;
      motor1_phd = 1;
      break;
    }
  }
}

void _ISRFAST _T4Interrupt(void)  // interrupt for motor 2 (of two) = right motor

{
  static int motor_phase=0;	// phase can be 0 to 3

  IFS1bits.T4IF = 0;             // clear interrupt flag

  // increment or decrement phase depending on direction

  if (right_speed > 0) // inverted for the two motors
    {
    nbr_pas_right++;
	motor_phase--;
    if (motor_phase < 0) motor_phase = 3;
    }
  else 
    {
    nbr_pas_right--;
    motor_phase++;
    if (motor_phase > 3) motor_phase = 0;
    }
  
  // set the phase on the port pins

  switch (motor_phase)
  {
    case 3:
    {
      motor2_pha = 1;
      motor2_phb = 0;
      motor2_phc = 1;
      motor2_phd = 0;
      break;
    }
    case 2:
    {
      motor2_pha = 1;
      motor2_phb = 0;
      motor2_phc = 0;
      motor2_phd = 1;
      break;
    }
    case 1:
    {
      motor2_pha = 0;
      motor2_phb = 1;
      motor2_phc = 0;
      motor2_phd = 1;
      break;
    }
    case 0:
    {
      motor2_pha = 0;
      motor2_phb = 1;
      motor2_phc = 1;
      motor2_phd = 0;
      break;
    }
  }
}

/* ---- user calls ---- */

int GetStepsLeft(void)
{
  return nbr_pas_left;
}

void SetStepsLeft(int set_steps)
{
  InterruptOFF();
  nbr_pas_left = set_steps;
  InterruptON();
}

int GetStepsRight(void)
{
  return nbr_pas_right;
}

void SetStepsRight(int set_steps)
{
  InterruptOFF();
  nbr_pas_right = set_steps;
  InterruptON();
}

void SetSpeedLeft(int motor_speed)  // motor speed in steps/s
{
  if (motor_speed == 0)
  {
    T5CONbits.TON = 0;            // stop Timer5
    motor1_pha = 0;
    motor1_phb = 0;
    motor1_phc = 0;
    motor1_phd = 0;
  }
  else
  {
    T5CONbits.TON = 0;            // stop Timer5

   left_speed = motor_speed;
   T5CON = 0;                    // 
   T5CONbits.TCKPS=3;            // prescsaler = 256
   TMR5 = 0;                     // clear timer 5
   PR5 = (FCY/256)/abs(motor_speed);  // interrupt every Xms with 256 prescaler
   IFS1bits.T5IF = 0;            // clear interrupt flag
   IEC1bits.T5IE = 1;            // set interrupt enable bit
   T5CONbits.TON = 1;            // start Timer5
  }
}


void SetSpeedRight(int motor_speed)  // motor speed in steps/s
{
  if (motor_speed == 0)
  {
    T4CONbits.TON = 0;   
 	motor2_pha = 0;
    motor2_phb = 0;
    motor2_phc = 0;
    motor2_phd = 0;         // stop Timer4
  }
  else
  {
    T4CONbits.TON = 0;            // stop Timer4

    right_speed = motor_speed;

  T4CON = 0;                    // 
   T4CONbits.TCKPS=3;            // prescsaler = 256
   TMR4 = 0;                     // clear timer 4
    PR4 = (FCY/256)/abs(motor_speed);  // interrupt every Xms with 256 prescaler
   IFS1bits.T4IF = 0;            // clear interrupt flag
   IEC1bits.T4IE = 1;            // set interrupt enable bit
    T4CONbits.TON = 1;            // start Timer4
  }
}


void InitMotors(void)
{
  InitPort();				// init general ports
}

/* internal calls for prox */
void InitTMR1(void)
{
  T1CON = 0;                    // 
  T1CONbits.TCKPS = 1;          // prescsaler = 8
  TMR1 = 0;                     // clear timer 1
  PR1 = (350.0*MICROSEC)/8.0;   // first interrupt after 350us with 8 prescaler
  IFS0bits.T1IF = 0;            // clear interrupt flag
  IEC0bits.T1IE = 1;            // set interrupt enable bit
  T1CONbits.TON = 1;            // start Timer1
}

void _ISRFAST _T1Interrupt(void)
{

// read ambient light and switch on leds in a first phase
// wait 350 us to let the phototransistor react
// read reflected light and switch off the leds in a second phase
// wait 3 ms before stating again
// repeat these two steps for the four couples of prox sensors

  static int ir_phase=0;	// phase can be 0 (ambient) or 1 (reflected)	
  static int ir_number=0;	// number goes from 0 to 3 (4 couples of sensors)	

  IFS0bits.T1IF = 0;            // clear interrupt flag

  switch (ir_number)
  {
    case 0:		// ir sensors 0 and 4
    {
      if (ir_phase == 0)
      {
        PR1 = (350.0*MICROSEC)/8.0;		// next interrupt in 350 us
		ambient_ir[0] = ReadAd(IR0);
        ambient_ir[4] = ReadAd(IR4);
        pulse_IR0 = 1;			// led on for next measurement
        ir_phase = 1;			// next phase
      }
      else
      {
        PR1 = (2100.0*MICROSEC)/8.0;		// next interrupt in 3 ms
        ambient_and_reflected_ir[0] = ReadAd(IR0);
        ambient_and_reflected_ir[4] = ReadAd(IR4);
		reflected_ir[0] = ambient_ir[0] - ambient_and_reflected_ir[0];
		reflected_ir[4] = ambient_ir[4] - ambient_and_reflected_ir[4];
        pulse_IR0 = 0;			// led off
        ir_phase = 0;			// reset phase
        ir_number = 1;			// next two sensors
      }
      break;
    }
    case 1:		// ir sensors 1 and 5
    {
      if (ir_phase == 0)
      {
        PR1 = (350.0*MICROSEC)/8.0;		// next interrupt in 350 us
        ambient_ir[1] = ReadAd(IR1);
        ambient_ir[5] = ReadAd(IR5);
        pulse_IR1 = 1;			// led on for next measurement
        ir_phase = 1;			// next phase
      }
      else
      {
        PR1 = (2100.0*MICROSEC)/8.0;		// next interrupt in 3 ms
        ambient_and_reflected_ir[1] = ReadAd(IR1);//读取第一号传感器
        ambient_and_reflected_ir[5] = ReadAd(IR5);
		reflected_ir[1] = ambient_ir[1] - ambient_and_reflected_ir[1];
		reflected_ir[5] = ambient_ir[5] - ambient_and_reflected_ir[5];
        pulse_IR1 = 0;			// led off
        ir_phase = 0;			// reset phase
        ir_number = 2;			// next two sensors
      }
      break;
    }
    case 2:		// ir sensors 2 and 6
    {
      if (ir_phase == 0)
      {
        PR1 = (350.0*MICROSEC)/8.0;		// next interrupt in 350 us
        ambient_ir[2] = ReadAd(IR2);
        ambient_ir[6] = ReadAd(IR6);
        pulse_IR2 = 1;			// led on for next measurement
        ir_phase = 1;			// next phase
      }
      else
      {
        PR1 = (2100.0*MICROSEC)/8.0;		// next interrupt in 3 ms
        ambient_and_reflected_ir[2] = ReadAd(IR2);
        ambient_and_reflected_ir[6] = ReadAd(IR6);
		reflected_ir[2] = ambient_ir[2] - ambient_and_reflected_ir[2];
		reflected_ir[6] = ambient_ir[6] - ambient_and_reflected_ir[6];
        pulse_IR2 = 0;			// led off
        ir_phase = 0;			// reset phase
        ir_number = 3;			// next sensor
      }
      break;
    }
    case 3:		// ir sensors 3 and 7
    {
      if (ir_phase == 0)
      {
        PR1 = (350.0*MICROSEC)/8.0;		// next interrupt in 350 us
        ambient_ir[3] = ReadAd(IR3);
        ambient_ir[7] = ReadAd(IR7);
        pulse_IR3 = 1;			// led on for next measurement
        ir_phase = 1;			// next phase
      }
      else
      {
        PR1 = (2100.0*MICROSEC)/8.0;		// next interrupt in 3 ms
        ambient_and_reflected_ir[3] = ReadAd(IR3);
        ambient_and_reflected_ir[7] = ReadAd(IR7);
		reflected_ir[3] = ambient_ir[3] - ambient_and_reflected_ir[3];
		reflected_ir[7] = ambient_ir[7] - ambient_and_reflected_ir[7];
        pulse_IR3 = 0;			// led off
        ir_phase = 0;			// reset phase
        ir_number = 0;			// next sensor (back to beginning)
      }
      break;
    }
  }
  
}



void InitProx(void)
	{
	InitAd();				// init AD converter module
	InitTMR1();				// init timer 1 for ir processing
	}
int GetProx(unsigned int sensor_number)
	{
	if (sensor_number > 7)
		return 0;
	else
		return reflected_ir[sensor_number];
	}

int GetAmbientLight(unsigned int sensor_number)
	{
	if (sensor_number > 7)
		return 0;
	else
		return ambient_ir[sensor_number];
	}
void goforward(int speed)
{  SetSpeedLeft(speed);
   SetSpeedRight(speed);
  

}
void left(int speed)
{SetSpeedLeft(0);
 SetSpeedRight(speed);
 
}
void right(int speed)
{SetSpeedLeft(speed);
 SetSpeedRight(0);
  
}
void stop()
{SetSpeedLeft(0);
 SetSpeedRight(0);
}
void back(int speed)
{  SetSpeedLeft(-speed);
   SetSpeedRight(-speed);

}
    
main()
{

int flag=3;
int lflag=3;
InitMotors();
InitProx();
InitPort();
InitTMR4();InitTMR5();
goforward(300);
while(1)
  {    if(( GetProx(0)>=600)&&(GetProx(7)>=600)){flag=0;}
       if(( GetProx(7)>=600)&&(GetProx(6)>=600)){flag=1;}
       if(( GetProx(5)>=600)&&(GetProx(6)>=600)){flag=1;}
       if(( GetProx(0)>=600)&&(GetProx(1)>= 600)){flag=2;}
       if(( GetProx(1)>=600)&&(GetProx(2)>= 600)){flag=2;}
       if(( GetProx(0)<600)&&(GetProx(7)<600)&&(GetProx(1)<600)&&(GetProx(6)<600)){flag=3;}
       switch (flag)
       {
         case 0:
          {     if(lflag!=flag)
                  {   long i;
                      back(300);
                      for(i=0;i<1000000;i++)
			          asm("nop");  
                      right(400);
                      for(i=0;i<1000000;i++)
			          asm("nop"); 
                      lflag=flag;
                     
                  }
          }
         case 1:
          {if(lflag!=flag){right(400);lflag=flag;}}
         case 2:
          {if(lflag!=flag){left(400);lflag=flag;}}
         case 3:
          {if(lflag!=flag){goforward(300);lflag=flag;}}
         }
 
}

}