main.c

speed controol of a dc motor

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <pic18.h>
#include "serial.h"

#define PWM_TICKS 1

/*Current Gain settings are:
 * Kp = 1600
 * Ki = 400
 */

#define INTEGRAL_CONSTANT (i_gain);
#define PROPORTIONAL_CONSTANT (p_gain);
#define DIFFERENTIAL_CONSTANT (d_gain);
#define JOINT_CONSTANT (j_gain);
#define ARC_CONSTANT (a_gain);
#define STOP_DEADBAND 1
#define PCONTROL_DEADBAND 5
#define MIN_SPEED 1
#define MAX_STEP 1

#define RIGHT_DIRECTION RA4
#define LEFT_DIRECTION RB5

/*Right = A = Bottom*/
#define RIGHT_PWM_H CCPR1L
#define RIGHT_PWM_M DC1B1
#define RIGHT_PWM_L DC1B0

/*Left = B = Top*/
#define LEFT_PWM_H CCPR2L
#define LEFT_PWM_M DC2B1
#define LEFT_PWM_L DC2B0

#define FORWARD_LEFT 1
#define BACKWARD_LEFT 0
#define FORWARD_RIGHT 0
#define BACKWARD_RIGHT 1
#define RIGHT_BRAKE RA2
#define LEFT_BRAKE RA3

#define DEBUG 0
#define DEBUG_ALL 0 
#define USE_SPI
#define USE_EEPROM

#define EEPROM_KP_ADDRESS 0
#define EEPROM_KI_ADDRESS 0x08
#define EEPROM_KD_ADDRESS 0x10
#define EEPROM_KJ_ADDRESS 0x18
#define EEPROM_KA_ADDRESS 0x20

#define SPEED_SET 5
#define DIST_SET 10
#define LEFT_SPEED_SET 15
#define RIGHT_SPEED_SET 20
#define LEFT_DIST_SET 25
#define RIGHT_DIST_SET 30
#define GENERAL_READ 35
#define SHIFTED_DIST 40
#define RESET_DIST 45

void LoopTime(void);
void Initialize(void);
void PwmInitialize(void);
void BlinkAlive(void);
void UpdatePWM(void);
void ManageAccel(void);
void puthex(unsigned int c);
void puthexc(unsigned char c);
void get_params(void);
void write_pwm_left(unsigned int duty_cycle);
void write_pwm_right(unsigned int duty_cycle);
void position_control(void);
void parse_SPI(void);
void set_left_position_target(long new_target);
void set_right_position_target(long new_target);
void write_float_to_eeprom( float value, unsigned int address );
void read_float_from_eeprom( float *value, unsigned int address );

/*JF had made all of these static*/
static long last_count_left = 0;
static long last_count_right = 0;
static long error_integral_left = 0;
static long error_integral_right = 0;
static long last_error_left = 0;
static long last_error_right = 0;
static float joint_integral_error = 0;

volatile long count_left = 0;
volatile long count_right = 0;
unsigned char portb_copy = 0;
volatile long count_left_reference = 0;
volatile long count_right_reference = 0;
volatile long count_left_reference_report = 0;
volatile long count_right_reference_report = 0;
float applied_right = 0;
float applied_left = 0;
long left_tick_target;
long right_tick_target;

char controlling_position = 0;
char position_slowdown = 0;

//SPI parsing flags
char newmessage = 0;
char setspeed = 0;
char setdist = 0;
int interrupt_speed_left;
int interrupt_speed_right;
int interrupt_dist_left;
int interrupt_dist_right;

/*Delete Between These*/
char printmessage = 0;
char spi_temp = 0;
char sawthis = 0;
/*Delete Between THese*/

int desired_target_right =0;
int desired_target_left = 0;
long target_right =0;
long target_left = 0;
float p_gain = 0.01;
float i_gain = 0.01;
float d_gain = 0.00;
float j_gain = 0.00;
float a_gain = 0.00;
volatile char rx_message[8];
char tx_message1[8];
char tx_message2[8];
char *current_tx_message = tx_message1;
char *newest_tx_message = tx_message1;

void main(void)
{
    unsigned char PWM_counter = 0;
    char c;

    TRISA = 0xFE; 
	//RA0 = 1;
    Initialize();
    PwmInitialize();

    serial_init();
	//RA0 = 1;

    while(1)
    { 	
        if(RA1 == 0 && kbhit())
        {
            if(kbhit())
            {
                c = getch();
            }
			if(c == 'q')
			{
				printf("QuickBug\n\r");
				asm("goto 0x6002");
			}
			else if(c == 'r')
            {
                desired_target_left = -desired_target_left;
                desired_target_right = -desired_target_right;
            }
			else if(c == 'a')
			{
				desired_target_left = -25;
				desired_target_right = 25;
			}
			else if(c == 'w')
			{
				desired_target_left = 25;
				desired_target_right = 25;
			}
			else if( c == 'd')
			{
				desired_target_left = 25;
				desired_target_right = -25;
			}
			else if(c == 'x')
			{
				desired_target_left = -25;
				desired_target_right = -25;
			}
            else if(c == 's')
            {
                desired_target_left = 0;
                desired_target_right = 0;
            }
            else if(c == 'b')
            {
                RA2 ^= 1;
				RA3 ^= 1;
                printf("Brakes: %d, %d", RA2, RA3);
            }
			else if(c == 'p')
			{
				set_left_position_target(0);
            	set_right_position_target(3375);	
			}
			else if(c == '[')
			{
				set_left_position_target(1687);
				set_right_position_target(-1687);
			}
			else if(c == ']')
			{
				set_left_position_target(-1687); 	
				set_right_position_target(1687);
			}
            else 
            {
                write_pwm_left(0);
                write_pwm_right(0);
                get_params();
                count_left = 0;
                count_right = 0;
                last_count_left = 0;
                last_count_right = 0;
                error_integral_left = 0;
                error_integral_right = 0;
                last_error_left = 0;
                last_error_right = 0;
            }
        }
        BlinkAlive();
        PWM_counter++;
        if(PWM_counter == PWM_TICKS)
        {
            PWM_counter = 0;
			if(sawthis)
			{
				putch('T');
				sawthis = 0;
			}
            if(printmessage)
            {
				printmessage = 0;
#if 0  
				putch('.');
                putch('\n'); 
				putch('\r');
                putch('[');
				for( i = 0; i < 8; i++ )
				{
					puthexc( rx_message[i] );
                    putch( ' ' );
				}
				putch(']');
                putch(' ');
#endif
				
                if( WCOL )
                {
                    putch( 'C' );
                    WCOL = 0;
                }
                if( SSPOV )
                {
                    putch( 'O' );
                    SSPOV = 0;
                }
            }
            parse_SPI();
            position_control();
            UpdatePWM();
            ManageAccel();
        }
        LoopTime();
    }
}

/* parse_SPI ******************************************************************
 * This function will run when the newmessage flag is non-zero.  When it runs,
 * parse_SPI reads the recieved message (rx_message) and carries out sets
 * requested by the controlling processor.
 * ***************************************************************************/
void parse_SPI(void)
{
    if (newmessage)
    {
		putch(newmessage);
		newmessage = 0;
		if(setspeed)
		{
			putch('s');	
			setspeed = 0;  //we've delt with speed update
			GIEL = 0;
			desired_target_left = interrupt_speed_left;
			desired_target_right = interrupt_speed_right;
			GIEL = 1;
			if((desired_target_left < 0) && (desired_target_right < 0))
			{
				putch('-');
			}
		}	
		if(setdist)
		{
			putch('d');
			setdist = 0; //we've delt with the distance update
			GIEL = 0;
            set_left_position_target(interrupt_dist_left);
            set_right_position_target(interrupt_dist_right);
			GIEL =1;
		}
    }
}


void set_left_position_target(long new_target)
{
	GIEH = 0;  //disable interrupts so we don't get wack reference data
    count_left_reference = count_left;
	GIEH = 1;  //reenable interrupts
    left_tick_target = new_target;
 	controlling_position |= 0x01;	
	
	if(((new_target > 0) && (desired_target_left < 0)) || 
	   ((new_target < 0) && (desired_target_left > 0)))
    {
        desired_target_left = -desired_target_left;
    }
}


void set_right_position_target(long new_target)
{
	GIEH = 0;  //disable interrupts so we don't get wack reference data
    count_right_reference = count_right;
	GIEH = 1;  //reenable interrupts
    right_tick_target = new_target;
	controlling_position |= 0x02;
	
    if(((new_target > 0) && (desired_target_right < 0)) || 
	   ((new_target < 0) && (desired_target_right > 0)))
    {
        desired_target_right = -desired_target_right;
    }
}

void position_control(void)
{
    static int posn_error_left;
    static int posn_error_right;
	
    if(controlling_position)
    {			
		GIEH = 0;
        posn_error_left = left_tick_target - ( count_left - count_left_reference ); 
        posn_error_right = right_tick_target - ( count_right - count_right_reference );
		GIEH = 1;

 		putch('\n');
		putch('\r');
		puthex(posn_error_left);
		putch(' ');
		puthex(posn_error_right);		
		putch('|');
		puthex(count_left);
		putch(' ');
		puthex(count_right);
		putch('|');
		puthex(desired_target_left);
		putch(' ');
		puthex(desired_target_right);
		putch('|');
				
		
		
		/*CONTROL LEFT WHEEL*/

		if(controlling_position & 0x01)
		{
			if((posn_error_left > 0 && desired_target_left < 0) || 
	  	 	   (posn_error_left < 0 && desired_target_left > 0))
   		 	{
				putch('<');
   		     	desired_target_left = -desired_target_left;
	   	 	}
			
			if((posn_error_left < (desired_target_left)) && 
			   (posn_error_left > -(desired_target_left)))
			{
				desired_target_left = 0;
				controlling_position &= 0xFE;
			}
		}
		else
		{
			desired_target_left = 0;
		}

		
		
		/*CONTROL RIGHT WHEEL*/

		if(controlling_position & 0x02)
		{
			if((posn_error_right > 0 && desired_target_right < 0) || 
			   (posn_error_right < 0 && desired_target_right > 0))
	    	{
				putch('>');
	        	desired_target_right = -desired_target_right;
	    	}
			if((posn_error_right < (desired_target_right)) && 
			   (posn_error_right > -(desired_target_right)))
			{
				desired_target_right = 0;
				controlling_position &= 0xFD;
			}
		}
		else
		{
			desired_target_right = 0;
		}
    }
}

void UpdatePWM(void)
{
	/*Remember for this section: 
	 * A = Bottom Port = Left
	 * B = Top Port = Right
	 */
		
    long saved_left;
    long saved_right;
    long diff_left;
    long diff_right;
    long error_left;
    long error_right;
	long distance_left;
	long distance_right;
	long report_distance_left;
	long report_distance_right;
	

    /* We need to disable interrupts here or we'll have a race
     * condition while copying these 2-byte values because the ISR
     * could jump in and change the value under us -- JF
     */
    GIEH = 0;
    saved_left = count_left;
    saved_right = count_right;
    GIEH = 1; 

	distance_left = saved_left - count_left_reference;
	distance_right = saved_right - count_right_reference;

	//This section smooths rapid changes in target speed, and sets
	//the target_left and target_right variables accordingly
	if(1)//(position_slowdown & 0x01) == 0)  //position isn't slowing left wheel
	{
		if((desired_target_left - target_left) > MAX_STEP)
		{
			target_left += MAX_STEP;
		}
		else if ((desired_target_left - target_left) < -MAX_STEP)
		{
			target_left -= MAX_STEP;
		}
		else
		{	
			target_left = desired_target_left;
		}
	}
	
	if(1)//(position_slowdown & 0x02) == 0)  //position isn't slowing right wheel
	{
		if((desired_target_right - target_right) > MAX_STEP)
		{
			target_right += MAX_STEP;
		}
		else if ((desired_target_right - target_right) < -MAX_STEP)
		{
			target_right -= MAX_STEP;
   	    }
	    else
	   	{
	   		target_right = desired_target_right;
	   	}
	}

    diff_left = saved_left - last_count_left;
    diff_right = saved_right - last_count_right;
	
#if 1 //DEBUG
    //putch('\r');
    //putch('\n');
		if(controlling_position){
	 	puthex(diff_left >> 16);
		puthex(diff_left);
	    putch(' ');
	    puthex(diff_right >> 16);
		puthex(diff_right);
	}
#endif

    error_left = target_left - diff_left;
    error_right = target_right - diff_right;
#if DEBUG_ALL
    putch('|');
    puthex(error_left);
    putch(' ');
    puthex(error_right);
#endif

    error_integral_left += error_left;
    error_integral_right += error_right;
	
	if(desired_target_right == desired_target_left && desired_target_right != 0 && desired_target_left != 0)
	{
		joint_integral_error += diff_left - diff_right + ARC_CONSTANT;
	}

    /******** Control Left Wheel ********/
    if( target_left == 0 && desired_target_left == 0 &&
		((diff_left >= -STOP_DEADBAND) && (diff_left <= STOP_DEADBAND)))
    {
        applied_left = error_integral_left * INTEGRAL_CONSTANT;
	}
    else 
    {   
        applied_left = error_integral_left * INTEGRAL_CONSTANT;
		applied_left += error_left * PROPORTIONAL_CONSTANT;
		applied_left += (error_left - last_error_left) * DIFFERENTIAL_CONSTANT;
		if(desired_target_left == desired_target_right)
		{   /*We will only add the straightness gain if we want to go straight*/
			applied_left -= joint_integral_error * JOINT_CONSTANT;
		}
    }
	
    if(applied_left > 1023)
    {
        applied_left = 1023;
    }
    else if(applied_left < -1023)
    {
        applied_left = -1023;
    }

    /******** Control Right Wheel ********/
    if(target_right == 0 && desired_target_right == 0 &&
		((diff_right >= -STOP_DEADBAND) && (diff_right <= STOP_DEADBAND)))
    {
        applied_right = error_integral_right * INTEGRAL_CONSTANT;
    }
    else
    {   
        applied_right = error_integral_right * INTEGRAL_CONSTANT;
        applied_right += error_right * PROPORTIONAL_CONSTANT;
        applied_right += (error_right - last_error_right) * DIFFERENTIAL_CONSTANT;
		if(desired_target_left == desired_target_right)
		{  /*We will only add the straightness gain if we want to go straight*/
			applied_right += joint_integral_error * JOINT_CONSTANT;
		}

    }
	
	if(applied_right > 1023)
    {
        applied_right = 1023;
    }
    else if(applied_right < -1023)
    {
        applied_right = -1023;
    }


    //Loads the outgoing status message
	/* Write into the tx buffer that isn't currently being transmitted */
	if( current_tx_message == tx_message1 )
	{
		newest_tx_message = tx_message2;
	}
	else
	{
		newest_tx_message = tx_message1;
	}
#if 0