rcservo.c

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

//  RC servo control   RCservo.c					// Rev 9/1/05

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

/*
  Provides for control of up to 7 RC servos. 
  
  The number of RCservo ports is specified by the NumSonars variable set by the
  laptop.  The number can vary from zero to 7.  Sonars and RC servos share the
  same input ports. Sonars must start at RC0 and go up. RCservos start when 
  servos end and continue up to RC6.
  
  Servo can be commanded in one of two modes:  Go to a specified angle, or scan
  back and forth between two specified angles.	The rate of turn is also 
  specified.  Also, the current commanded position can be read by RCservopos().
  
  void RCservoGoTo(int num, int cmd, int rate)
    num:  selects the specific RC servo output (0 to 6)
	cmd:  specifies the servo angle in degrees
	rate: specifies the rate of turn in degrees per second
	
  void RCservoScan(int num, int max, int min, int rate)
    same as above, plus...
	max:  most positive angle end of scan
	min:  most negative angle end of scan	
  
  int RCservopos (int num)  
    num:  selects the specific RC servo output (0 to 6)
    returns:  current commanded angle in degrees
	
	note:  the servo position is not accurate if the "rate" command exceeds 
	the servos capability. 
	
  RCservo must be called every real time cycle to update the servo positions.	 										
*/

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

// external variables
extern int RC_Sonar[7];					// data for output on USB bus

//local variables
int RCservoposn[7];  //store current servo position
int servocmd[7];
int servorate[7];
int servomax[7];
int servomin[7];
int dir[7];		   //scan direction.  0 = CCW, 1 = CW
// RC servo calibration factors for gain and offset
int RCcfgGain0,RCcfgGain1,RCcfgGain2,RCcfgGain3,RCcfgGain4,RCcfgGain5,RCcfgGain6;
int RCcfgOff0,RCcfgOff1,RCcfgOff2,RCcfgOff3,RCcfgOff4,RCcfgOff5,RCcfgOff6;

//inputs
extern int time;
extern int NumSonars;
 
//return current commanded position
int RCservopos (int num)  
  { return RCservoposn[num];
  }
  
//set servo to go to a specific position and stop
void RCservoGoTo(int num, int rate, int cmd)
  { if(cmd >  900) cmd =  900;	   //limit command to +/- 90 degrees
	if(cmd < -900) cmd = -900;
	servocmd[num] = cmd;
    servorate[num] = rate;
	servomax[num] = 9999;  //set invalid
    servomin[num] = 9999;  //set invalid
printf("RCgoto %d %d %d\n",num,rate,cmd);
    return;
  }	 
    
//set servo to scan back and forth between 2 points
void RCservoScan(int num, int rate, int max, int min)
  { if(max >  900) max =  900;	//limit range to +/- 90 degrees
	if(max < -900) max = -900;
	if(min >  900) min =  900;
	if(min < -900) min = -900;
	servomax[num] = max;
    servomin[num] = min;
    servorate[num] = rate;
	servocmd[num] = 9999; //set invalid
	dir[num] = 1; 		  //initial direction is clockwise
	return;
  }	 

//real time servo adjustment
void RCservo()
  { int i;	  	  			//loop counter
    int scmd;				//temporary servo command
	long delscmd;			//delta servo command
	static long temp[7];	//long servo command

  	for(i = NumSonars;i<=6;i++)
	  	{ if(servocmd[i] != 9999)  //if in RCServoGoTo mode
	       { scmd = servocmd[i];
		     delscmd = (long)scmd*61 - temp[i];
			 if(delscmd >  servorate[i]) delscmd = servorate[i];
			 if(delscmd < -servorate[i]) delscmd = -servorate[i];
			 temp[i] += delscmd;
		     RCservoposn[i] = temp[i]/61;
//if(i==4)printf("Goto %d  %d  %d  %ld  %ld  %d  \n",i,servorate[i],dir[i],delscmd,temp[i],RCservoposn[i]);
		   } //end of RCServoGoTo mode
		  else if(servomax[i] != 9999)  //if in servoScan mode
	       { scmd = RCservoposn[i];
		     delscmd = (long)servorate[i];
		     if(dir[i] == 0) delscmd = -delscmd;
			 temp[i] += delscmd;
			 if(temp[i] > (long)servomax[i]*61) temp[i] = (long)servomax[i]*61;
			 if(temp[i] < (long)servomin[i]*61) temp[i] = (long)servomin[i]*61;
			 RCservoposn[i] = temp[i]/61;
		     if(RCservoposn[i] >= servomax[i]) dir[i] = 0; //reverse dir when
		     if(RCservoposn[i] <= servomin[i]) dir[i] = 1; //  reach limits
		    }  //end of servoScan mode
			
			 RC_Sonar[i] = RCservoposn[i];	   		  	 // set output variable
	   }      //end of for
	   	   
		   //set pulse widths (2250 -> 1500 usec.
			   TC3 = 2250 - (int)((temp[0]+RCcfgOff0)*RCcfgGain0/40667); //RC 0
			   TC4 = 2250 - (int)((temp[1]+RCcfgOff1)*RCcfgGain1/40667); //RC 1
			   TC5 = 2250 - (int)((temp[2]+RCcfgOff2)*RCcfgGain2/40667); //RC 2
			   TC6 = 2250 - (int)((temp[3]+RCcfgOff3)*RCcfgGain3/40667); //RC 3
			   TC0 = 2250 - (int)((temp[4]+RCcfgOff4)*RCcfgGain4/40667); //RC 4
			   TC1 = 2250 - (int)((temp[5]+RCcfgOff5)*RCcfgGain5/40667); //RC 5
			   TC2 = 2250 - (int)((temp[6]+RCcfgOff6)*RCcfgGain6/40667); //RC 6
		return;
	} //end of RCservo
	
//  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.
*/