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_DATAmc_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().

00213                                {
00214   float pos_rads = mc_data.get_position();
00215   fixed pos = float_to_fixed(pos_rads);
00216   fixed vel = float_to_fixed(mc_data.get_velocity());
00217   fixed control = mc_command_current - fixed_mult(mc_c1, pos) - fixed_mult(mc_c2, vel);
00218   mc_set_target_current_fixed(control);
00219   mc_pid_current();
00220 }

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.

00279 {
00280   int dir = direction/* * mc_data.operation*/; //if running backwards, opposite direction
00281   if (dir == MC_POSITIVE){
00282     mc_positive = command;
00283   } else if (dir == MC_NEGATIVE){
00284     mc_negative = command;
00285   }
00286 }

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.

00476 { 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.

00309 {
00310   return &mc_data;
00311 } 

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.

00318 {
00319   return mc_pwm;
00320 }

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().

00641                                                   {
00642   //Check max current for thermal shutoff
00643   //Must create current_factor, thermal_safe, thermal_limit
00644   
00645   static fixed multiplier = FIXED_ONE;
00646   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
00647   // DANGEREROUS TO INITIALIZE A to zero, eg after lettting the robo on for a while, then switching off and then turning on again
00648   
00649   fixed a_dot_dt;  //derivative of 'a' multiplied by time step dt
00650   fixed motor_current = current;
00651   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
00652   a = a + a_dot_dt;   //  
00653  
00654   
00655   if (a < mc_data.thermal_safe){ //below limit, full current
00656     multiplier = FIXED_ONE;
00657     if (mc_therm_off){ //previously turned off by thermal limit, but safe again
00658       mc_turnon(MC_THERM); //turn on controller
00659     }
00660   } else if (a >= mc_data.thermal_safe && a  < mc_data.thermal_limit){ //above safety limit, use mult factor
00661     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))
00662     error_occurred(ERROR_MC_TEMP_SAFE);   
00663   } else if (a >= mc_data.thermal_limit){ //above strict limit, shutdown
00664     mc_turnoff(MC_THERM); //turn off controller due to thermal limit
00665     multiplier = 0;
00666   } else {
00667     multiplier = FIXED_ONE; //should never reach here
00668   }
00669     
00670   return multiplier;
00671  
00672 } 

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.

00133 {
00134   mc_data.operation = op;
00135   
00136   mc_data.motor_voltage_max = max_volts;  //Volts 
00137   mc_data.current_max = float_to_fixed(max_current);  //Amps
00138   mc_data.current_min = float_to_fixed(min_current);  //Amps
00139   mc_data.target_max = float_to_fixed(max_target);
00140   mc_data.target_min = float_to_fixed(min_target);
00141   
00142   mc_data.thermal_limit = float_to_fixed((thermal_current_limit*thermal_current_limit)*thermal_time_constant);
00143   mc_data.tau_inv = float_to_fixed(1.0/thermal_time_constant);
00144   mc_data.thermal_safe = fixed_mult(float_to_fixed(0.75), mc_data.thermal_limit);
00145   mc_data.thermal_diff = float_to_fixed(1.0/(thermal_current_limit - fixed_to_float(mc_data.thermal_safe)));
00146   mc_data.dt = float_to_fixed(1.0/(float)(1000*SCHED_SPEED));
00147   
00148   mc_data.kp = float_to_fixed(kp);
00149   mc_data.ki = float_to_fixed(ki);
00150   mc_data.kd = float_to_fixed(kd);  
00151   mc_data.integral = 0;
00152   mc_data.max_pwm = max_pwm;
00153   mc_data.max_integral = float_to_fixed((((float)max_pwm)/ki));
00154   mc_data.error_limit  = error_limit;
00155   mc_data.smart_error_limit = float_to_fixed(smart_error_limit);
00156   mc_data.smart_error_limit_inverse = float_to_fixed(1.0*10.0/smart_error_limit); 
00157   mc_data.get_current = current;
00158   mc_data.get_position = position;
00159   mc_data.get_velocity = velocity;
00160 }

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().

00295                               {
00296   int running = (!mc_off) && (!mc_sleep) && (!mc_shutdown) && (!mc_therm_off) && (!mc_mech_off);
00297   if (!running)error_occurred(ERROR_MC_NOT_RUNNING);
00298   return running;
00299 }

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().

00487 {
00488   volatile fixed mc_error = 0;  
00489   static unsigned char start_count = 2;
00490   signed long int curr_pwm = 0;
00491   long long int pid_sum, p_component, i_component;
00492   fixed thermal_multiplier, mechanical_multiplier;
00493   
00494   mc_update_watchdog(); //feed watchdog if running or sleeping
00495 
00496   if (!start_count){ //wait a few iterations to allow the adcx to get meaningful data
00497     
00498     fixed current = mc_data.get_current(); //motor current on the robot
00499     fixed target_current = mc_target_current; //the target current      
00500     thermal_multiplier = mc_get_thermal_limiter(current); //thermal multiplier, est. temp and limit current
00501     mechanical_multiplier = mc_smart_check_current(current);
00502   
00503     if (mc_is_running()){
00504       // ******************************* calc prop and integral **************************
00505       //Calculate the error:
00506       mc_error = (target_current - current);
00507       //Calculate integral if no direction control active
00508       if (mc_negative && mc_positive){
00509         mc_data.integral += mc_error;
00510       }
00511       
00512       // ****************** check for saturation of integral constant ********************
00513       if (mc_data.integral > mc_data.max_integral){
00514         mc_data.integral = mc_data.max_integral; 
00515         error_occurred(ERROR_MC_INTEG_SATUR);
00516       } else if (mc_data.integral < -1*mc_data.max_integral){
00517         mc_data.integral = -1*mc_data.max_integral;
00518         error_occurred(ERROR_MC_INTEG_SATUR);
00519       }
00520       
00521       // Sum the terms of the PID algorithm to give the new PWM duty cycle value
00522       //Note: Need to bound each component such that their sum can't overflow
00523       p_component = fixed_mult_to_long((mc_error),(mc_data.kp));
00524       i_component = fixed_mult_to_long((mc_data.integral),(mc_data.ki));
00525       pid_sum = p_component + i_component;
00526     
00527       // check for fixed point overflow
00528       if(pid_sum > FIXED_MAX) {
00529         pid_sum = FIXED_MAX;
00530         error_occurred(ERROR_MC_FIXED_LIMIT);
00531       } else if (pid_sum < -1*FIXED_MAX) {
00532         pid_sum = -1*FIXED_MAX;
00533         error_occurred(ERROR_MC_FIXED_LIMIT);
00534       }
00535       
00536       if(thermal_multiplier < mechanical_multiplier){ //use whichever multiplier is smaller
00537         pid_sum = fixed_mult(pid_sum, thermal_multiplier);
00538       } else {
00539         pid_sum = fixed_mult(pid_sum, mechanical_multiplier);
00540       }
00541       
00542       curr_pwm = fixed_to_int((pid_sum));
00543           
00544       // ********************** check if PWM is within limits ****************************
00545       if (curr_pwm > mc_data.max_pwm){
00546         curr_pwm = mc_data.max_pwm;
00547         error_occurred(ERROR_MC_PWM_LIMIT);
00548       }
00549       else if (curr_pwm < -1*mc_data.max_pwm){
00550         curr_pwm = -1*mc_data.max_pwm;
00551         error_occurred(ERROR_MC_PWM_LIMIT);
00552       }
00553           
00554       mc_set_pwm(curr_pwm); 
00555       
00556     } else {
00557       mc_set_pwm(0);
00558     }
00559   } else {
00560     start_count--;
00561   } 
00562 }

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().

00571 {
00572   if (mc_is_running()){
00573     mc_update_watchdog();
00574 //    mc_check_current(mc_data.get_current());
00575   }
00576 }

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().

00228 {
00229   mc_command_current = float_to_fixed(new_command);
00230   mc_save_comm = new_command;
00231   //if (new_command != 7.2){mcu_led_red_blink(10);}
00232   mc_c1 = mc_new_c1;
00233   mc_c2 = mc_new_c2;
00234 }

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().

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

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().

00420 {
00421   const unsigned long int full = PWMMR0;
00422   const unsigned long int offset = 14;
00423 
00424   if (!mc_is_running()){ //controller is asleep, shutdown, or off
00425     pwm = 0;
00426   } else if (pwm < 0) { //trying to go in negative direction
00427     pwm = pwm * mc_negative;
00428   } else if (pwm > 0) { //trying to go in positive direction, but not allowed
00429     pwm = pwm * mc_positive;
00430   }  
00431   
00432   pwm = mc_data.operation * pwm;
00433 
00434   mc_pwm = pwm;
00435   
00436   mc_update_watchdog();
00437   
00438   //check bounds
00439   if (pwm > mc_data.max_pwm){
00440     pwm = mc_data.max_pwm;
00441     error_occurred(ERROR_MC_PWM_LIMIT);
00442   }
00443   else if (pwm < -1*mc_data.max_pwm){
00444     pwm = -1*mc_data.max_pwm;
00445     error_occurred(ERROR_MC_PWM_LIMIT);
00446   }
00447   
00448   if (mc_phase == 0){ //Normal operation: PWMA = x%, PWMB = 0%
00449     if(pwm < 0) { //Negative current, clockwise  
00450       PWMMR2 = 0;       //Set duty cycle of PWM2 to 0 so that only one motor direction is on
00451       PWMMR6 = -1 * pwm;        //Set duty cycle of PWM6 (out of 600 max)
00452       PWMLER = (1<<2)|(1<<6);   //Transfer new PWMMR2 & PWMMR6 values to reg at next PWM match
00453     }
00454     else { //Positive current, counter-clockwise
00455       PWMMR2 = pwm;  //Set duty cycle of PWM2 (out of 600 max)  
00456       PWMMR6 = 0;   //Set duty cycle of PWM6 to 0 so that only one motor direction is on    
00457       PWMLER = (1<<2)|(1<<6);     //Transfer new PWMMR2 & PWMMR6 values to reg at next PWM match
00458     }
00459     mc_phase = 1;
00460   } else { //Alternate Operation: PWMA = 100%, PWMB = 100-x%
00461     if(pwm < 0) {//negative current, clockwise
00462       PWMMR2 = full - (pwm * -1) - offset;
00463       PWMMR6 = full;
00464       PWMLER = (1<<2)|(1<<6);
00465     } else { //positive current, counter-clockwise
00466       PWMMR2 = full;
00467       PWMMR6 = full - (pwm) - offset;
00468       PWMLER = (1<<2)|(1<<6);
00469     }
00470     mc_phase = 0;
00471   }
00472 }

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.

00349                                   {
00350   unsigned long sd = ((unsigned long)shutdown) & 0x7FFF;
00351   mc_shutdown = sd;
00352 }

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.

00360                             {
00361   unsigned long sl = ((unsigned long)sleep) & 0x7FFF;
00362   mc_sleep = sl;
00363 }

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().

00244                                    {
00245   mc_new_c1 = float_to_fixed(new_c1); 
00246   mc_save_stiff = new_c1;
00247   //if (new_c1 != 4.0){mcu_led_red_blink(10);}
00248 }

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().

00168 {
00169   fixed curr = float_to_fixed(new_current);
00170   mc_set_target_current_fixed(curr);
00171 }

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().

00179                                                       {
00180 //  fixed curr = mc_data.operation * current;
00181   fixed curr = current;
00182   if (curr < mc_data.target_min){
00183     error_occurred(ERROR_MC_TCURR_OOB);
00184     curr = mc_data.target_min;
00185   }
00186   if (curr > mc_data.target_max){
00187     error_occurred(ERROR_MC_TCURR_OOB);
00188     curr = mc_data.target_max;
00189   }
00190   mc_target_current = curr;
00191 }

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().

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

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().

00588                                                   {
00589   //Check max current for mechanical shutoff
00590   //Must change error_limit to around 20.
00591   static fixed current_error = 0;
00592   fixed motor_current = current;
00593   static fixed multiplier = FIXED_ONE;
00594   
00595   // we'll set the limit to 4.5 amps.. if it is below these limits then the things are fine.
00596   // as soon as we try to go above this limit a multiplier less than one will be generated.  
00597   // ****** Integrate Error ****** //
00598   if (motor_current > 0){
00599     current_error = current_error + (motor_current - mc_data.current_max);
00600   } else if (motor_current < 0){
00601     current_error = current_error - (motor_current - mc_data.current_min);
00602   }
00603   if (current_error <= 0){  
00604     current_error = 0;
00605     if (mc_mech_off){ //previously turned off due to mechanical limit
00606       mc_turnon(MC_MECH); //integrator <= 0 so safe to turn back on
00607     }
00608   }
00609 
00610   // ****** Calculate multiplier ****** //
00611   multiplier = FIXED_ONE - (fixed_mult(current_error , mc_data.smart_error_limit_inverse));
00612   if (multiplier < 0){multiplier = 0;}
00613   mc_mult = fixed_to_float(multiplier);
00614   // 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
00615   // instead I wanted the multiplier to decay faster becasue its taking time to respond/ or the error is growing very fast.
00616   // so I am settign it to be like 1/.1/smart_error_limit so that it goes to zero quicker and then remains zero
00617   
00618   // ****** Check absolute limit ****** //
00619   if (current_error >= mc_data.smart_error_limit){ //set limit to ~10, can be changed.
00620     mc_turnoff(MC_MECH);
00621   }
00622   //this is so that when curren go to dangerous level of 8 
00623   //then the error 8-4.5 = 3.5 will have about three tic tocs to reach to 10 and stops.
00624   //3.3 ms is the mechanichal time constant.  
00625         
00626   return multiplier;
00627                      
00628 }

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().

00372                                               {
00373   switch(reason){ //set appropriate state and cause error
00374     case MC_THERM: mc_therm_off = 1;
00375       error_occurred(ERROR_MC_TEMP_OFF);
00376       break;
00377     case MC_MECH: mc_mech_off = 1;
00378       error_occurred(ERROR_MC_MECH_OFF);
00379       break;
00380     default: mc_off = 1;
00381       error_occurred(ERROR_MC_SHUTOFF);
00382       break;
00383   }
00384   
00385   mc_pwm = 0;
00386   PWMMR2 = 0;   //Both PWMs set to 0
00387   PWMMR6 = 0;
00388   PWMLER = (1<<6)|(1<<2);
00389   
00390   mcu_led_green_on();
00391 }

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().

00400                                              {
00401   switch(reason){ //set appropriate state and cause error
00402     case MC_THERM: mc_therm_off = 0;
00403       error_occurred(ERROR_MC_TEMP_ON);
00404       break;
00405     case MC_MECH: mc_mech_off = 0;
00406       error_occurred(ERROR_MC_MECH_ON);
00407       break;
00408     default: //shouldn't get here
00409       break;
00410   }
00411   
00412   mcu_led_green_off();
00413 }

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().

00331 {
00332   if (!mc_shutdown && !mc_off && !mc_therm_off && !mc_mech_off){
00333     if(FIO0PIN & (1<<6)){
00334       FIO0CLR = (1<<6); //make watchdog low
00335     }
00336     else {
00337       FIO0SET = (1<<6); //set watchdog hi
00338     }
00339   }
00340 }

Generated on Tue Jun 29 16:36:15 2010 by  doxygen 1.6.3