motor_controller.c File Reference

Motor controlling functions for use on the B2A motor controller boards. More...

Go to the source code of this file.

Functions
void	mc_compliant_control (void)
	A compliant controller for the motor.
void	mc_direction_control (MOTOR_DIRECTION direction, MOTOR_CONTROL command)
	Controls the motor in a given direction.
float	mc_get_command_current (void)
float	mc_get_dampness (void)
float	mc_get_mult (void)
	Returns the current multiplier for either thermal or mech current limiting.
MC_DATA *	mc_get_parameters (void)
	Gets the parameters currently being used by the motor controller.
int	mc_get_pwm (void)
	Gets the current PWM being sent to the motor.
float	mc_get_stiffness (void)
static fixed	mc_get_thermal_limiter (fixed current)
	Calculates a thermal limiting multiplier for use by mc_pid_current.
void	mc_init (float max_volts, float max_current, float min_current, float max_target, float min_target, float thermal_current_limit, float thermal_time_constant, float kp, float ki, float kd, int max_pwm, int error_limit, float smart_error_limit, MC_OPERATION op, FIXED_VOID_F current, FLOAT_VOID_F position, FLOAT_VOID_F velocity)
	Initializes the motor controller.
static int	mc_is_running (void)
	Returns whether the motor is running or not.
void	mc_pid_current (void)
	Uses a PID controller to try and reach the target current.
void	mc_run_no_control (void)
	Runs the motor with no control.
void	mc_set_command_current (float new_command)
	Sets the command current used by mc_compliant_control to new_command.
void	mc_set_dampness (float new_c2)
	Sets the dampness used by mc_compliant_control to new_c2.
void	mc_set_pwm (int pwm)
	Sets the PWM signal going to the motor to pwm.
void	mc_set_shutdown (int shutdown)
	Sets the shutdown state of the motor.
void	mc_set_sleep (int sleep)
	Sets the sleep state of the motor.
void	mc_set_stiffness (float new_c1)
	Sets the stiffness used by mc_compliant_control to new_c1.
void	mc_set_target_current (float new_current)
	Sets the target current of the PID current controller to new_current.
static void	mc_set_target_current_fixed (fixed current)
	Sets the target current of the PID current controller to current as a fixed point value.
void	mc_set_unsafe_target_current (float new_current)
	Sets the target current to new_current without any limiting.
static fixed	mc_smart_check_current (fixed current)
	Checks the motor current for limits.
static void	mc_turnoff (MC_SAFETY_CAUSE reason)
	Turns off the motor controller until it is safe to turn back on.
static void	mc_turnon (MC_SAFETY_CAUSE reason)
	Turns on the motor controller once it is safe to turn back on.
void	mc_update_watchdog (void)
	Updates the motor controllers watchdog timer.
Variables
static volatile fixed	mc_c1 = 0
static volatile fixed	mc_c2 = 0
static volatile fixed	mc_command_current = 0
static MC_DATA	mc_data
static volatile unsigned long int	mc_direction = 0
static volatile unsigned long int	mc_mech_off = 0
static float	mc_mult
static long int	mc_negative = 1
static volatile fixed	mc_new_c1 = 0
static volatile fixed	mc_new_c2 = 0
static volatile unsigned long int	mc_off = 0
static unsigned long int	mc_phase
static long int	mc_positive = 1
static volatile signed long int	mc_pwm = 0
static float	mc_save_comm
static float	mc_save_damp
static float	mc_save_stiff
static volatile unsigned long int	mc_shutdown = 0
static volatile unsigned long int	mc_sleep = 0
static volatile fixed	mc_target_current = 0
static volatile unsigned long int	mc_therm_off = 0

---

Detailed Description

Motor controlling functions for use on the B2A motor controller boards.

Example Hardware and Register setup for PWM control of motors:

  // *******************************************************************************
  // Motor Controller Setup
  // *******************************************************************************
  //Motor control initialization
  //FIO0DIR |= (1<<9);    // set P0.9 to be output (PWM A)
  //FIO0DIR |= (1<<7);     // set P0.7 to be output (PWM B)
  FIO0DIR |= (1<<11);     // set P0.11 to be output (Low side enable A)
  FIO1DIR |= (1<<17);     // set P1.17 to be output (Low side enable B)
  FIO0DIR |= (1<<6);      // set P0.6 to be output (Watchdog timer) 
  //FIO0CLR = (1<<9);   // turn off PWM A output
  //FIO0CLR = (1<<7);   // turn off PWM B output
  FIO0SET = (1<<11);    // turn on low side A enable output
  FIO1SET = (1<<17);    // turn on low side B enable output
  FIO0CLR = (1<<6);   // turn off watchdog timer output
  //Initializes the PWM registers and PWM2 and PWM6 outputs
  //Pin Function Select
  PINSEL0 &= ~(3<<14);
  PINSEL0 |= (2<<14);  //PWM 2 on Pin P0.7 Enabled (bits 14/15)
  PINSEL0 &= ~(3<<18);
  PINSEL0 |= (2<<18);  //PWM 6 on Pin P0.9 Enabled (bits 18/19)
  //PWM Prescale Register. Controls number of PClK cycles per Timer Counter Cycle
  PWMPR= 0; //Will run at maximum rate
  //PWM Match Registers
  //PWM Match Register 0. Resets Timer Counter and sets all PWM outputs to 1.
  PWMMR0= 600; //100 kHz PWM frequency (60000000/600 = 100,000)
  PWMMR1= 0;
  PWMMR2= 0;
  PWMMR3= 0;
  PWMMR4= 0;
  PWMMR5= 0;
  PWMMR6= 0;
  //PWM Match Control Register
  PWMMCR= (PWMMCR|0x2) & ~PWMMCR_RB; //Should set Timer Counter to reset upon reaching Match Register 0
  //PWM Timer Control Register.
  PWMTCR= (PWMTCR|0x1) // Enable Timer Counter
      | (PWMTCR|0x8) // Enable PWM Mode
      & ~PWMTCR_RB; 
               //Must Occur after Initialization of Match Register 0.
  //PWM Control Register. Enables individual PWM types and outputs.
  PWMPCR= (PWMPCR|0x4400) & ~PWMPCR_RB; //Should mean the same thing as next 4 lines (PWM Control Register)
  //PWMSEL2= 0; //Enables Single Edge PWM 2 Control
  //PWMSEL6= 0; //Enables Single Edge PWM 6 Control
  //PWMENA2= 1; //PWM 2 Output Enabled
  //PWMENA6= 1; //PWM 6 Output Enabled
  //PWM Latch Enable Register. Updates Match values.
  //Each bit directly relates to a Match Register
  //PWMLER= 0;  //Latch Register to zero
  PWMMR2= 0;       // out of 600  -ve   
  PWMMR6= 0;       // out of 600  +ve        
  // Latch enable reg automatically cleared once PWM match values are set
  PWMLER= (PWMLER|0x45) & ~PWMLER_RB;  //Enable update of PWM match registers 0, 2 and 6.  0x45 --> bit0, bit2, bit6

Author:

Nicolas Williamson

Date:

5/18/09.

Definition in file motor_controller.c.

---

Function Documentation

void mc_compliant_control

(

void

)

A compliant controller for the motor.

Similar to a PD-position controller. To use, the user should call mc_set_stiffness, mc_set_dampness, and mc_set_command_current to set the parameters of the controller. A call to mc_set_command_current will latch the new stiffness and dampness values; without this call, dampness and stiffness (and the command current) won't change. The formula used in the control is

 Icontrol = Icommand - (stiffness * pos) - (dampness * vel) 

. Icontrol is passed to the PID current controller, which will attempt to go to the desired current. To request a specific position (in radians) with a given stiffness, use:

 Icommand = stiffness * desired_position 

.

Definition at line 213 of file motor_controller.c.

References fixed_mult(), float_to_fixed(), MC_DATA::get_position, MC_DATA::get_velocity, mc_pid_current(), and mc_set_target_current_fixed().

                               {
  float pos_rads = mc_data.get_position();
  fixed pos = float_to_fixed(pos_rads);
  fixed vel = float_to_fixed(mc_data.get_velocity());
  fixed control = mc_command_current - fixed_mult(mc_c1, pos) - fixed_mult(mc_c2, vel);
  mc_set_target_current_fixed(control);
  mc_pid_current();
}

void mc_direction_control	(	MOTOR_DIRECTION	direction,
		MOTOR_CONTROL	command
	)

Controls the motor in a given direction.

The direction, either MC_POSITIVE or MC_NEGATIVE, is defined as the direction a PWM signal with that sign causes the motor to turn. The command is either for the motor to stop running in that direction or to continue moving in that direction. If the motor is stopped in a given direction, it may continue to move in the opposite direction, provided that direction hasn't also been stopped. Note that the directions may be controlled independently of one another.

Parameters:

	direction	The direction, either MC_POSITIVE or MC_NEGATIVE, in which to control the motor.
	command	The command, either MC_STOP or MC_START.

Definition at line 278 of file motor_controller.c.

{
  int dir = direction/* * mc_data.operation*/; //if running backwards, opposite direction
  if (dir == MC_POSITIVE){
    mc_positive = command;
  } else if (dir == MC_NEGATIVE){
    mc_negative = command;
  }
}

float mc_get_mult

(

void

)

Returns the current multiplier for either thermal or mech current limiting.

Definition at line 476 of file motor_controller.c.

{ return mc_mult;}

MC_DATA* mc_get_parameters

(

void

)

Gets the parameters currently being used by the motor controller.

This may be used to read or change the parameters values.

Returns:

A pointer to the MC_DATA struct used to hold most of the motor controller's parameters.

Deprecated:

Definition at line 308 of file motor_controller.c.

{
  return &mc_data;
} 

int mc_get_pwm

(

void

)

Gets the current PWM being sent to the motor.

Returns:

The current PWM.

Definition at line 317 of file motor_controller.c.

{
  return mc_pwm;
}

static fixed mc_get_thermal_limiter

(

fixed

current

)

[static]

Calculates a thermal limiting multiplier for use by mc_pid_current.

Using the thermal_current_limit and thermal_time_constant passed to mc_init, this function will estimate the current temperature of the motor and find a multiplier that when multiplied by the current will limit the temperature. If the motor is too hot, this function will call mc_turnoff to completely shutdown the motor until it cools down.

Warning:

This function uses the motor current and Newton's Law of Cooling to estimate the temperature, it doesn't measure the motor temperature directly, so care should be taken to let the motors cool down when the robot is turned off and on, as the residual heat of the motors will not be taken into account.

Parameters:

current

The newest motor current to add to the thermal checking.

Definition at line 641 of file motor_controller.c.

References MC_DATA::dt, error_occurred(), fixed_mult(), mc_turnoff(), mc_turnon(), MC_DATA::tau_inv, MC_DATA::thermal_diff, MC_DATA::thermal_limit, and MC_DATA::thermal_safe.

Referenced by mc_pid_current().

                                                  {
  //Check max current for thermal shutoff
  //Must create current_factor, thermal_safe, thermal_limit
  
  static fixed multiplier = FIXED_ONE;
  static fixed a = 0; // a is  C/R*(temperature - environmental temperature), C is heat capacity of motor, R is electric resitance which causes the heating I^2*R
  // DANGEREROUS TO INITIALIZE A to zero, eg after lettting the robo on for a while, then switching off and then turning on again
  
  fixed a_dot_dt;  //derivative of 'a' multiplied by time step dt
  fixed motor_current = current;
  a_dot_dt = fixed_mult((fixed_mult(motor_current, motor_current)-fixed_mult(mc_data.tau_inv, a)), mc_data.dt); // derivative is -(1/tau)*a + I^2 , this is newton's law of cooling
  a = a + a_dot_dt;   //  
 
  
  if (a < mc_data.thermal_safe){ //below limit, full current
    multiplier = FIXED_ONE;
    if (mc_therm_off){ //previously turned off by thermal limit, but safe again
      mc_turnon(MC_THERM); //turn on controller
    }
  } else if (a >= mc_data.thermal_safe && a  < mc_data.thermal_limit){ //above safety limit, use mult factor
    multiplier = FIXED_ONE - (fixed_mult(a - mc_data.thermal_safe, mc_data.thermal_diff));  //mc.thermal_diff = float_to_fixed(1.0/(thermal_limit - thermal_safe))
    error_occurred(ERROR_MC_TEMP_SAFE);   
  } else if (a >= mc_data.thermal_limit){ //above strict limit, shutdown
    mc_turnoff(MC_THERM); //turn off controller due to thermal limit
    multiplier = 0;
  } else {
    multiplier = FIXED_ONE; //should never reach here
  }
    
  return multiplier;
 
} 

void mc_init	(	float	max_volts,
		float	max_current,
		float	min_current,
		float	max_target,
		float	min_target,
		float	thermal_current_limit,
		float	thermal_time_constant,
		float	kp,
		float	ki,
		float	kd,
		int	max_pwm,
		int	error_limit,
		float	smart_error_limit,
		MC_OPERATION	op,
		FIXED_VOID_F	current,
		FLOAT_VOID_F	position,
		FLOAT_VOID_F	velocity
	)

Initializes the motor controller.

Must be called before the motor controller module can be used properly.

Parameters:

	max_volts	The maximum voltage the motor can take.
	max_current	The maximum allowed current for the motor.
	min_current	The minimum allowed current for the motor, often just -1 * max_current.
	max_target	The maximum allowed current to be commanded using mc_set_target_current.
	min_target	The minimum allowed current to be commanded using mc_set_target_current.
	thermal_current_limit	The nominal or max continuous current for the motor.
	thermal_time_constant	The Winding Thermal Time Constant.
	kp	Proportional constant for the PID current controller.
	ki	Integral constant for the PID current controller.
	kd	Derivative constant for the PID current controller.
	max_pwm	Maximum allowed PWM for the motor, often to limit voltage.
	error_limit	If there are this many errors in a row, shutdown the motor. (DEPRECATED)
	smart_error_limit	If the integral of the error is greater than this value, shutdown the motor.
	op	The operation of the motor, either MC_NORMAL or MC_BACKWARDS. Used if positive PWM and positive position don't match.
	current	A function that returns the motor current as a fixed point value in amps.
	position	A function that returns the motor position as a floating point number in radians.
	velocity	A function that returns the motor velocity as a floating point number in radians.

Definition at line 116 of file motor_controller.c.

References MC_DATA::current_max, MC_DATA::current_min, MC_DATA::dt, MC_DATA::error_limit, fixed_mult(), fixed_to_float(), float_to_fixed(), MC_DATA::get_current, MC_DATA::get_position, MC_DATA::get_velocity, MC_DATA::integral, MC_DATA::kd, MC_DATA::ki, MC_DATA::kp, MC_DATA::max_integral, MC_DATA::max_pwm, MC_DATA::motor_voltage_max, MC_DATA::operation, MC_DATA::smart_error_limit, MC_DATA::smart_error_limit_inverse, MC_DATA::target_max, MC_DATA::target_min, MC_DATA::tau_inv, MC_DATA::thermal_diff, MC_DATA::thermal_limit, and MC_DATA::thermal_safe.

{
  mc_data.operation = op;
  
  mc_data.motor_voltage_max = max_volts;  //Volts 
  mc_data.current_max = float_to_fixed(max_current);  //Amps
  mc_data.current_min = float_to_fixed(min_current);  //Amps
  mc_data.target_max = float_to_fixed(max_target);
  mc_data.target_min = float_to_fixed(min_target);
  
  mc_data.thermal_limit = float_to_fixed((thermal_current_limit*thermal_current_limit)*thermal_time_constant);
  mc_data.tau_inv = float_to_fixed(1.0/thermal_time_constant);
  mc_data.thermal_safe = fixed_mult(float_to_fixed(0.75), mc_data.thermal_limit);
  mc_data.thermal_diff = float_to_fixed(1.0/(thermal_current_limit - fixed_to_float(mc_data.thermal_safe)));
  mc_data.dt = float_to_fixed(1.0/(float)(1000*SCHED_SPEED));
  
  mc_data.kp = float_to_fixed(kp);
  mc_data.ki = float_to_fixed(ki);
  mc_data.kd = float_to_fixed(kd);  
  mc_data.integral = 0;
  mc_data.max_pwm = max_pwm;
  mc_data.max_integral = float_to_fixed((((float)max_pwm)/ki));
  mc_data.error_limit  = error_limit;
  mc_data.smart_error_limit = float_to_fixed(smart_error_limit);
  mc_data.smart_error_limit_inverse = float_to_fixed(1.0*10.0/smart_error_limit); 
  mc_data.get_current = current;
  mc_data.get_position = position;
  mc_data.get_velocity = velocity;
}

static int mc_is_running

(

void

)

[static, private]

Returns whether the motor is running or not.

Returns:

The status of the motor.

• 0: if the motor is off, asleep, or shutdown
• 1: otherwise

Definition at line 295 of file motor_controller.c.

References error_occurred().

Referenced by mc_pid_current(), mc_run_no_control(), and mc_set_pwm().

                              {
  int running = (!mc_off) && (!mc_sleep) && (!mc_shutdown) && (!mc_therm_off) && (!mc_mech_off);
  if (!running)error_occurred(ERROR_MC_NOT_RUNNING);
  return running;
}

void mc_pid_current

(

void

)

Uses a PID controller to try and reach the target current.

The target currect used by this function should be set using mc_set_target_current or mc_set_target_current_fixed. The constants for the current controller are given by the kp, ki, and kd given to mc_init. Updates the watchdog, and uses both a thermal limiting multiplier and integrating current limits.

Definition at line 486 of file motor_controller.c.

References error_occurred(), fixed_mult(), fixed_mult_to_long(), fixed_to_int(), MC_DATA::get_current, MC_DATA::integral, MC_DATA::ki, MC_DATA::kp, MC_DATA::max_integral, MC_DATA::max_pwm, mc_get_thermal_limiter(), mc_is_running(), mc_set_pwm(), mc_smart_check_current(), and mc_update_watchdog().

Referenced by mc_compliant_control().

{
  volatile fixed mc_error = 0;  
  static unsigned char start_count = 2;
  signed long int curr_pwm = 0;
  long long int pid_sum, p_component, i_component;
  fixed thermal_multiplier, mechanical_multiplier;
  
  mc_update_watchdog(); //feed watchdog if running or sleeping

  if (!start_count){ //wait a few iterations to allow the adcx to get meaningful data
    
    fixed current = mc_data.get_current(); //motor current on the robot
    fixed target_current = mc_target_current; //the target current      
    thermal_multiplier = mc_get_thermal_limiter(current); //thermal multiplier, est. temp and limit current
    mechanical_multiplier = mc_smart_check_current(current);
  
    if (mc_is_running()){
      // ******************************* calc prop and integral **************************
      //Calculate the error:
      mc_error = (target_current - current);
      //Calculate integral if no direction control active
      if (mc_negative && mc_positive){
        mc_data.integral += mc_error;
      }
      
      // ****************** check for saturation of integral constant ********************
      if (mc_data.integral > mc_data.max_integral){
        mc_data.integral = mc_data.max_integral; 
        error_occurred(ERROR_MC_INTEG_SATUR);
      } else if (mc_data.integral < -1*mc_data.max_integral){
        mc_data.integral = -1*mc_data.max_integral;
        error_occurred(ERROR_MC_INTEG_SATUR);
      }
      
      // Sum the terms of the PID algorithm to give the new PWM duty cycle value
      //Note: Need to bound each component such that their sum can't overflow
      p_component = fixed_mult_to_long((mc_error),(mc_data.kp));
      i_component = fixed_mult_to_long((mc_data.integral),(mc_data.ki));
      pid_sum = p_component + i_component;
    
      // check for fixed point overflow
      if(pid_sum > FIXED_MAX) {
        pid_sum = FIXED_MAX;
        error_occurred(ERROR_MC_FIXED_LIMIT);
      } else if (pid_sum < -1*FIXED_MAX) {
        pid_sum = -1*FIXED_MAX;
        error_occurred(ERROR_MC_FIXED_LIMIT);
      }
      
      if(thermal_multiplier < mechanical_multiplier){ //use whichever multiplier is smaller
        pid_sum = fixed_mult(pid_sum, thermal_multiplier);
      } else {
        pid_sum = fixed_mult(pid_sum, mechanical_multiplier);
      }
      
      curr_pwm = fixed_to_int((pid_sum));
          
      // ********************** check if PWM is within limits ****************************
      if (curr_pwm > mc_data.max_pwm){
        curr_pwm = mc_data.max_pwm;
        error_occurred(ERROR_MC_PWM_LIMIT);
      }
      else if (curr_pwm < -1*mc_data.max_pwm){
        curr_pwm = -1*mc_data.max_pwm;
        error_occurred(ERROR_MC_PWM_LIMIT);
      }
          
      mc_set_pwm(curr_pwm); 
      
    } else {
      mc_set_pwm(0);
    }
  } else {
    start_count--;
  } 
}

void mc_run_no_control

(

void

)

Runs the motor with no control.

User should set the PWM explicitly with mc_set_pwm. Updates the watchdog timer and checks the current limits.

Note:

Should only be used in debugging.

Definition at line 570 of file motor_controller.c.

References mc_is_running(), and mc_update_watchdog().

{
  if (mc_is_running()){
    mc_update_watchdog();
//    mc_check_current(mc_data.get_current());
  }
}

void mc_set_command_current

(

float

new_command

)

Sets the command current used by mc_compliant_control to new_command.

Also latches new values for stiffness and dampness.

Parameters:

new_command

The command current for the compliant controller.

Definition at line 227 of file motor_controller.c.

References float_to_fixed().

{
  mc_command_current = float_to_fixed(new_command);
  mc_save_comm = new_command;
  //if (new_command != 7.2){mcu_led_red_blink(10);}
  mc_c1 = mc_new_c1;
  mc_c2 = mc_new_c2;
}

void mc_set_dampness

(

float

new_c2

)

Sets the dampness used by mc_compliant_control to new_c2.

This value will not be used (latched) until mc_set_command_current has been called.

Parameters:

new_c2

The new dampness for the compliant controller.

Definition at line 258 of file motor_controller.c.

References float_to_fixed().

                                  {
  mc_new_c2 = float_to_fixed(new_c2); 
  mc_save_damp = new_c2;
  //if (new_c2-0.2 > 0.01 || new_c2-0.2 < -0.01){mcu_led_red_blink(10);}else{mcu_led_green_blink(10);}
}

void mc_set_pwm

(

int

pwm

)

Sets the PWM signal going to the motor to pwm.

Parameters:

pwm

The PWM going to the motor. -1*max_pwm &lt pwm &lt max_pwm

Definition at line 419 of file motor_controller.c.

References error_occurred(), MC_DATA::max_pwm, mc_is_running(), mc_update_watchdog(), and MC_DATA::operation.

Referenced by mc_pid_current().

{
  const unsigned long int full = PWMMR0;
  const unsigned long int offset = 14;

  if (!mc_is_running()){ //controller is asleep, shutdown, or off
    pwm = 0;
  } else if (pwm < 0) { //trying to go in negative direction
    pwm = pwm * mc_negative;
  } else if (pwm > 0) { //trying to go in positive direction, but not allowed
    pwm = pwm * mc_positive;
  }  
  
  pwm = mc_data.operation * pwm;

  mc_pwm = pwm;
  
  mc_update_watchdog();
  
  //check bounds
  if (pwm > mc_data.max_pwm){
    pwm = mc_data.max_pwm;
    error_occurred(ERROR_MC_PWM_LIMIT);
  }
  else if (pwm < -1*mc_data.max_pwm){
    pwm = -1*mc_data.max_pwm;
    error_occurred(ERROR_MC_PWM_LIMIT);
  }
  
  if (mc_phase == 0){ //Normal operation: PWMA = x%, PWMB = 0%
    if(pwm < 0) { //Negative current, clockwise  
      PWMMR2 = 0;       //Set duty cycle of PWM2 to 0 so that only one motor direction is on
      PWMMR6 = -1 * pwm;        //Set duty cycle of PWM6 (out of 600 max)
      PWMLER = (1<<2)|(1<<6);   //Transfer new PWMMR2 & PWMMR6 values to reg at next PWM match
    }
    else { //Positive current, counter-clockwise
      PWMMR2 = pwm;  //Set duty cycle of PWM2 (out of 600 max)  
      PWMMR6 = 0;   //Set duty cycle of PWM6 to 0 so that only one motor direction is on    
      PWMLER = (1<<2)|(1<<6);     //Transfer new PWMMR2 & PWMMR6 values to reg at next PWM match
    }
    mc_phase = 1;
  } else { //Alternate Operation: PWMA = 100%, PWMB = 100-x%
    if(pwm < 0) {//negative current, clockwise
      PWMMR2 = full - (pwm * -1) - offset;
      PWMMR6 = full;
      PWMLER = (1<<2)|(1<<6);
    } else { //positive current, counter-clockwise
      PWMMR2 = full;
      PWMMR6 = full - (pwm) - offset;
      PWMLER = (1<<2)|(1<<6);
    }
    mc_phase = 0;
  }
}

void mc_set_shutdown

(

int

shutdown

)

Sets the shutdown state of the motor.

If the motor is shutdown, any calls to mc_set_pwm will be forced to 0, mc_pid_current will not calculate PWMs, and mc_update_watchdog will NOT update the watchdog, causing it to starve and shut off the motor controller in hardware.

Parameters:

shutdown

If this value is 0, the motor is not shutdown, else shutdown the motor.

Definition at line 349 of file motor_controller.c.

                                  {
  unsigned long sd = ((unsigned long)shutdown) & 0x7FFF;
  mc_shutdown = sd;
}

void mc_set_sleep

(

int

sleep

)

Sets the sleep state of the motor.

If the motor is asleep, it will continue to update the watchdog, but will not run the pid current controller and any attempt to use mc_set_pwm will be forced to 0.

Parameters:

sleep

0 = Awake, not 0 = Asleep.

Definition at line 360 of file motor_controller.c.

                            {
  unsigned long sl = ((unsigned long)sleep) & 0x7FFF;
  mc_sleep = sl;
}

void mc_set_stiffness

(

float

new_c1

)

Sets the stiffness used by mc_compliant_control to new_c1.

This value will not be used (latched) until mc_set_command_current has been called.

Parameters:

new_c1

The new stiffness for the compliant controller.

Definition at line 244 of file motor_controller.c.

References float_to_fixed().

                                   {
  mc_new_c1 = float_to_fixed(new_c1); 
  mc_save_stiff = new_c1;
  //if (new_c1 != 4.0){mcu_led_red_blink(10);}
}

void mc_set_target_current

(

float

new_current

)

Sets the target current of the PID current controller to new_current.

The current controller will attempt to achieve this current on the next call to mc_pid_current.

Parameters:

new_current

The target current for the PID current controller. target_min &lt new_current &lt target_max.

Definition at line 167 of file motor_controller.c.

References float_to_fixed(), and mc_set_target_current_fixed().

{
  fixed curr = float_to_fixed(new_current);
  mc_set_target_current_fixed(curr);
}

static void mc_set_target_current_fixed

(

fixed

current

)

[static]

Sets the target current of the PID current controller to current as a fixed point value.

The current controller will attempt to achieve this current on the next call to mc_pid_current. This function is faster than mc_set_target_current, and its use is preferred.

Parameters:

current

The target current in fixed point.

Definition at line 179 of file motor_controller.c.

References error_occurred(), MC_DATA::target_max, and MC_DATA::target_min.

Referenced by mc_compliant_control(), and mc_set_target_current().

                                                      {
//  fixed curr = mc_data.operation * current;
  fixed curr = current;
  if (curr < mc_data.target_min){
    error_occurred(ERROR_MC_TCURR_OOB);
    curr = mc_data.target_min;
  }
  if (curr > mc_data.target_max){
    error_occurred(ERROR_MC_TCURR_OOB);
    curr = mc_data.target_max;
  }
  mc_target_current = curr;
}

void mc_set_unsafe_target_current

(

float

new_current

)

Sets the target current to new_current without any limiting.

Parameters:

new_current

The new unlimited target current.

Definition at line 197 of file motor_controller.c.

References float_to_fixed().

{
  fixed curr = float_to_fixed(new_current);
  mc_target_current = /*mc_data.operation * */curr;
}

static fixed mc_smart_check_current

(

fixed

current

)

[static]

Checks the motor current for limits.

If the motor current is outside of the current_limits, this returns a multiplier which will be multiplied by the target current to slowly decrease its value. Also integrates the motor current error (the amount the motor current is above the current_limit), and if this value is greater than smart_error_limit, calls mc_turnoff to completely turn off the motor, until it is safely back within bounds.

Parameters:

current

The newest current to add to the smart current limiting.

Returns:

A multiplier to limit the current if it is out of bounds.

Definition at line 588 of file motor_controller.c.

References MC_DATA::current_max, MC_DATA::current_min, fixed_mult(), fixed_to_float(), mc_turnoff(), mc_turnon(), MC_DATA::smart_error_limit, and MC_DATA::smart_error_limit_inverse.

Referenced by mc_pid_current().

                                                  {
  //Check max current for mechanical shutoff
  //Must change error_limit to around 20.
  static fixed current_error = 0;
  fixed motor_current = current;
  static fixed multiplier = FIXED_ONE;
  
  // we'll set the limit to 4.5 amps.. if it is below these limits then the things are fine.
  // as soon as we try to go above this limit a multiplier less than one will be generated.  
  // ****** Integrate Error ****** //
  if (motor_current > 0){
    current_error = current_error + (motor_current - mc_data.current_max);
  } else if (motor_current < 0){
    current_error = current_error - (motor_current - mc_data.current_min);
  }
  if (current_error <= 0){  
    current_error = 0;
    if (mc_mech_off){ //previously turned off due to mechanical limit
      mc_turnon(MC_MECH); //integrator <= 0 so safe to turn back on
    }
  }

  // ****** Calculate multiplier ****** //
  multiplier = FIXED_ONE - (fixed_mult(current_error , mc_data.smart_error_limit_inverse));
  if (multiplier < 0){multiplier = 0;}
  mc_mult = fixed_to_float(multiplier);
  // smart_erroe_limit_inverse is not exactly 1/smart_error_limit, which would mean multiplier goes from one to zero when error goes from 0 to error limit
  // instead I wanted the multiplier to decay faster becasue its taking time to respond/ or the error is growing very fast.
  // so I am settign it to be like 1/.1/smart_error_limit so that it goes to zero quicker and then remains zero
  
  // ****** Check absolute limit ****** //
  if (current_error >= mc_data.smart_error_limit){ //set limit to ~10, can be changed.
    mc_turnoff(MC_MECH);
  }
  //this is so that when curren go to dangerous level of 8 
  //then the error 8-4.5 = 3.5 will have about three tic tocs to reach to 10 and stops.
  //3.3 ms is the mechanichal time constant.  
        
  return multiplier;
                     
}

static void mc_turnoff

(

MC_SAFETY_CAUSE

reason

)

[static, private]

Turns off the motor controller until it is safe to turn back on.

Sets the appropriate state and causes an error. Sets the PWM to 0 and lets the watchdog timer starve out. Also turns on the green LED.

Parameters:

reason

Why this function was called.

Definition at line 372 of file motor_controller.c.

References error_occurred().

Referenced by mc_get_thermal_limiter(), and mc_smart_check_current().

                                              {
  switch(reason){ //set appropriate state and cause error
    case MC_THERM: mc_therm_off = 1;
      error_occurred(ERROR_MC_TEMP_OFF);
      break;
    case MC_MECH: mc_mech_off = 1;
      error_occurred(ERROR_MC_MECH_OFF);
      break;
    default: mc_off = 1;
      error_occurred(ERROR_MC_SHUTOFF);
      break;
  }
  
  mc_pwm = 0;
  PWMMR2 = 0;   //Both PWMs set to 0
  PWMMR6 = 0;
  PWMLER = (1<<6)|(1<<2);
  
  mcu_led_green_on();
}

static void mc_turnon

(

MC_SAFETY_CAUSE

reason

)

[static, private]

Turns on the motor controller once it is safe to turn back on.

Sets the appropriate state and causes an error. Starts feeding the watchdog timer again. Also turns off the green LED.

Parameters:

reason

Why this function was called.

Definition at line 400 of file motor_controller.c.

References error_occurred().

Referenced by mc_get_thermal_limiter(), and mc_smart_check_current().

                                             {
  switch(reason){ //set appropriate state and cause error
    case MC_THERM: mc_therm_off = 0;
      error_occurred(ERROR_MC_TEMP_ON);
      break;
    case MC_MECH: mc_mech_off = 0;
      error_occurred(ERROR_MC_MECH_ON);
      break;
    default: //shouldn't get here
      break;
  }
  
  mcu_led_green_off();
}

void mc_update_watchdog

(

void

)

Updates the motor controllers watchdog timer.

This needs to be called frequently, otherwise the motor controller will shut itself off. It is automatically called from mc_pid_current, mc_run_no_control, and mc_set_pwm, so the user shouldn't have to explicitly call this unless they don't use any of those functions. The watchdog will not update if the motor controller is shutdown or off.

Definition at line 330 of file motor_controller.c.

Referenced by mc_pid_current(), mc_run_no_control(), and mc_set_pwm().

{
  if (!mc_shutdown && !mc_off && !mc_therm_off && !mc_mech_off){
    if(FIO0PIN & (1<<6)){
      FIO0CLR = (1<<6); //make watchdog low
    }
    else {
      FIO0SET = (1<<6); //set watchdog hi
    }
  }
}