init.c

老外开发的机器人的底层单片机代码。比较有参考价值哦！

//  Initialization     Init.c					// Rev 12/31/05

//Copyright (C) 2005 Alex Brown	[e-mail]
//This program is free software; See license at the end of this file for details.

#include "Microcontroller.h"		    //for registers & config

extern int NumSonars;
extern int servomax[7];
extern int servocmd[7];
extern int USBsendidx;
extern int MotorType;	   					//hbridge = 0, RC mode = 1 else -1
//-------------------------------------------------------------------------
// ATD initialization
//  Initializes both atd converters for continuous conversion in 10 bit mode.
//  Just read the result register(s) at any time to get the current value. 
// 	To ensure correct data, read both registers simultaneously. e.g. ATD0DR0
//  Output values will vary from 0x0000 to 0x03FF

void atdinit()
  { ATD0CTL2 = 0x80; 	 //normal atd function (converter 0)
    ATD0CTL3 = 0x40;	 //convert all 8 channels
	ATD0CTL4 = 0x23;	 //10 bit mode, 4 atd clocks,  /8 prescale
	ATD0CTL5 = 0xB0;	 //right justify (10 bit), continuous conversions
	
	ATD1CTL2 = 0x80; 	 //normal atd function (converter 1)
    ATD1CTL3 = 0x40;	 //convert all 8 channels
	ATD1CTL4 = 0x23;	 //10 bit mode, 4 atd clocks,  /8 prescale
	ATD1CTL5 = 0xB0;	 //right justify (10 bit), continuous conversions
  }	
	   
//Motor and encoder initialization
  /*  Sets up portP bits 0-2 as PWM outputs with a frequency of 15000 hz and a
  	  duty cycle of 0 to 100% is PWMDTY = 0 to 100( 0 is off, 100 full on)
	  This initialization is suitable for standard H-Bridge operation and for
	  RC type motor control.  If RC type, the initialization will be revised
	  in the Process Cmds module when the config message is received.
	  
     PORTP   0  Motor PWM cmd  2
   	 		 1		  	  	   1
			 2				   0
			 3  not used
			 4	Encoder 2 chan B
			 5	Encoder 0 chan A
			 6	Encoder 0 chan B
			 7  not used
			 
	 PORTH   0  Encoder 2  chan B	
	  		 1	Encoder 2  chan A	
			 2  Motor direction 0
			 3  Motor Brake     0
			 4  Motor direction 1
			 5  Motor Brake     1
			 6  Motor direction 2
			 7  Motor Brake     2 
			 
	 IRQ	    Encoder 2 chan  A		 
  */	 		 
  
void motorinit() 
/* This function initializes for either a standard h-bridge output or for an 
   RC servo output.  RC operation requires jumpers between pins H1 18/19 and 
   20/21. When this function is called, MotorType must have already been set 
   after receiving command message 1000.
*/
{
  if     (MotorType == 1) printf("// MotorType = 1 (RC)\n");
  else if(MotorType == 0) printf("// MotorType = 0 (H bridge)\n");
  else                    printf("// Motor type invalid");
   
   if(MotorType == 1)   //for motor RC operation
    { 
    PWMPRCLK = 0x00;  	  //Set clock A & B prescales to /1
    PWMCLK   = 0x0F;	  //Select clock SA for PWM chans 0,1 & SB for 2,3
	PWMSCLA  = 12;		  //Set scaled clocks to 1 MHz.
	PWMSCLB  = 12;		  //i.e.  24 MHz / 12 / 2 = 1 MHz
	PWMCTL   = 0x30;	  //concatenate channels 0/1 and 2/3
	PWMPER0  = 80;		  //Set periods to 20.48 milliseconds.  Arbitrary value
	PWMPER1  = 0;		  // to exceed normal real time frame of 16.67 msec.
	PWMPER2  = 80;
	PWMPER3  = 0;
	PWMDTY0  = 0;  	  	  //init duty cycles to zero
	PWMDTY1  = 0;
	PWMDTY2  = 0;
	PWMDTY3  = 0;
	PWMPOL   = 0x0A;	  //start new duty cycles high for pwm channels
	PWME     = 0x0A;	  //enable PWM channels 
   }
   
   if(MotorType == 0)		  // H bridge operation
   { 
    PWMPRCLK = 0x44;  	  //Set clock A & B prescales to /16
    PWMCLK   = 0x00;	  //Select clock A for PWM chans 0,1 & B for 2
	PWMPER0  = 100;		  //Set periods to 250 =  6 KHz (noisy!)
	PWMPER1  = 100;		  //               100 = 15 KHz
	PWMPER2  = 100;
	PWMDTY0  = 0;  		  //init duty cycles to zero
	PWMDTY1  = 0;
	PWMDTY2  = 0;
	PWMPOL   = 0x07;	  //start new duty cycles high for pwm channels
	PWME     = 0x07;	  //enable PWM channels    
   }
  	
	// Set up encoder inputs
	PIEP = 0x20;	//bit 5 will cause PortP interrupt	 	 //Encoder 0
	PPSP = 0x20;    //bit 5 cause interrupt on rising edge
	
	DDRH = 0xFC;	//bits 0,1 as inputs, bits 2-7 as outputs
	PPSH = 0x02;	//bit 1 causes interrupt on rising edge  //Encoder 1
	PIEH = 0x02;	//bit 1 causes PortH interrupt
	
	INTCR = 0xC0;	//IRQ is enabled to cap on falling edge	 //Encoder 2
  }
  
//-------------------------------------------------------------------------
// Timer initialization
/*  
    TC0  used for Sonar or RCservo 4
	TC1  used for Sonar or RCservo 5
	TC2  used for Sonar or RCservo 6
	TC3  used for Sonar or RCservo 0
	TC4  used for Sonar or RCservo 1
	TC5  used for Sonar or RCservo 2
	TC6  used for Sonar or RCservo 3
	
	for RC servos set TCTL1/2 to "10" for each channel and "00" for sonars. 
	TCTL1 chan 7 should be set to "10" for timer reset
	TCTL3/4 should be "10" for sonar. "00" for RC (and OC7).
	TIOS should be set to "1" for each RC (and OC7), else "0"
	TOC7M and TOC7D should be "1" for each RC, else "0"
	Set TCx to 2250 for each RC. 
*/  
void TimerInit()
  { int i;				//loop counter
  
    switch (NumSonars)
	{
	  case 0:  // 0 sonars, 7 RC servo
		 TCTL1 = 0xAA; TCTL2 = 0xAA; TCTL3 = 0x00; TCTL4 = 0x00;
		 TIOS  = 0xFF; TOC7M = 0x7F; TOC7D = 0x7F;
		 TC3 = TC4 = TC5 = TC6 = TC0 = TC1 = TC2 = 2250;  //1.5 msec(center)
		 break;
		 
	  case 1:  // 1 sonar,  6 RC servos
		 TCTL1 = 0xAA; TCTL2 = 0x2A; TCTL3 = 0x00; TCTL4 = 0x80;
		 TIOS  = 0xF7; TOC7M = 0x77; TOC7D = 0x77;
		 TC4 = TC5 = TC6 = TC0 = TC1 = TC2 = 2250;  	 //1.5 msec(center)
		 break;
		 
	  case 2:  // 2 sonars,  5 RC servos
		 TCTL1 = 0xA8; TCTL2 = 0x2A; TCTL3 = 0x02; TCTL4 = 0x80;
		 TIOS  = 0xE7; TOC7M = 0x67; TOC7D = 0x67;
		 TC5 = TC6 = TC0 = TC1 = TC2 = 2250;  	   		 //1.5 msec(center)
		 break;
		 
	  case 3:  // 3 sonars,  4 RC servos
		 TCTL1 = 0xA0; TCTL2 = 0x2A; TCTL3 = 0x0A; TCTL4 = 0x80;
		 TIOS  = 0xC7; TOC7M = 0x47; TOC7D = 0x47;
		 TC6 = TC0 = TC1 = TC2 = 2250;	     	   		 //1.5 msec(center)
		 break;
		 
	  case 4:  // 4 sonars,  3 RC servos
		 TCTL1 = 0x80; TCTL2 = 0x2A; TCTL3 = 0x2A; TCTL4 = 0x80;
		 TIOS  = 0x87; TOC7M = 0x07; TOC7D = 0x07;
		 TC0 = TC1 = TC2 = 2250;  	   		 	   		 //1.5 msec(center)
		 break;
		 
	  case 5:  // 5 sonars,  2 RC servos
		 TCTL1 = 0x80; TCTL2 = 0x28; TCTL3 = 0x2A; TCTL4 = 0x82;
		 TIOS  = 0x86; TOC7M = 0x06; TOC7D = 0x06;
		 TC1 = TC2 = 2250; 	   		 	    	   		 //1.5 msec(center)
		 break;
		 
	  case 6:  // 6 sonars,  1 RC servos
		 TCTL1 = 0x80; TCTL2 = 0x20; TCTL3 = 0x2A; TCTL4 = 0x8A;
		 TIOS  = 0x84; TOC7M = 0x04; TOC7D = 0x04;
		 TC2 = 2250;	     	   		   	 	   		 //1.5 msec(center)
		 break;
		 
	  case 7:  // 7 sonars,  0 RC servos
		 TCTL1 = 0x80; TCTL2 = 0x00; TCTL3 = 0x2A; TCTL4 = 0xAA;
		 TIOS  = 0x80; TOC7M = 0x00; TOC7D = 0x00;
		 break;
	}	 
		 
    TSCR2 = 0x0C;       //overflow intr inhibited, timer reset on OC7,
		  				//   prescale = 16
	TC7   = 30000;		//20 ms period
 	TSCR1 = 0x80; 		//turn timer on
	
	//set up ports for sonar trigger outputs 
	//                (set as if all sonars since otherwise unused)
	DDRA |= 0xF0;
	DDRK |= 0xB0;

		for(i=0;i<=6;i++)
	  { servomax[i] = 9999;  //set both servo modes invalid for each servo
	    servocmd[i] = 9999; 
	  }
  }	

  
//-------------------------------------------------------------------------
// Accel port initialization
/*  
    Port J   0    used for x/y accel
	Port J   1    used for x/y accel
	Port J   2-5  not available
	Port J   6/7  used for IIC
*/  
void accelinit()
  {
    DDRJ = 0x00;	//bits 0/1 as inputs. Enabling IIC will control 6/7
    PIEJ = 0x03;	//bits 0 and 1 will cause PortJ interrupt
	PPSJ = 0x03;    //bits 0 and 1 cause interrupt on rising edge
  }	
  
//  OPEN SOURCE SOFTWARE LICENSE
/* Permission is hereby granted, free of charge, to any person obtaining a copy 
of this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to use, 
copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 
Software, and to permit persons to whom the Software is furnished to do so, 
subject to the following conditions:

The above copyright notice and this permission notice shall be included in all 
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/