mdv_control_modes.c
No OneTemporary
Actions

Subscribers

None

File Metadata

Created: Thu, Nov 28, 12:03

mdv_control_modes.c
View Options

	#include "mdv_control_modes.h"

	#include "mdv_speed_pid.h"

	#include "misc_math.h"

	#include "cos_table.h"

	#include "sbcp_mdv.h"


	void mdv_coast_init(motordriver * mdv){
	gpio_set(mdv->pins.brake);
	gpio_clear(mdv->pins.coast);
	}

	void mdv_brake_init(motordriver * mdv){
	gpio_set(mdv->pins.coast);
	gpio_clear(mdv->pins.brake);

	}

	void mdv_position_process(motordriver * mdv){
	if(mdv_flag(mdv,MDV_F_NEW_POSITION_AVAILABLE)){
	mdv->goal_pos = mdv_e2i_position(mdv,mdv->user_goal_pos);

	mdv->goal_pos = clamp(mdv->goal_pos,0,mdv->pos_limit);
	mdv->goal_speed = clamp_absolute(mdv_lo2hi_res_speed(mdv->goal_pos - mdv->moving_pos) / mdv->params.smooth_interval ,mdv->max_speed );

	int abs_speed = abs(mdv->goal_speed);

	//mdv->goal_pos = mdv->moving_pos + (mdv->goal_pos - mdv->moving_pos)*(long)(SMOOTH_POSITION_OVERSHOOT + 1 );

	//disable moving during play with parameter
	mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE);
	//checks that the set speed is correct !!!!!! users could be morrons
	// and move motor if needed
	if(mdv->goal_pos != mdv-> moving_pos ) {
	if (mdv->goal_pos > mdv-> moving_pos) {
	mdv->goal_speed = abs_speed;
	} else {
	mdv->goal_speed = -abs_speed;
	}
	mdv_set_flag(mdv,MDV_F_MOVING_ENABLE);
	}
	mdv_clear_flag(mdv,MDV_F_NEW_POSITION_AVAILABLE);
	}
	//now process normally
	mdv_speed_pid_with_compliance_process(mdv);
	}

	void mdv_position_init(motordriver * mdv){
	gpio_set(mdv->pins.coast);
	gpio_set(mdv->pins.brake);

	}

	void mdv_velocity_process(motordriver * mdv){
	if(mdv_flag(mdv,MDV_F_NEW_SPEED_AVAILABLE)){

	mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE);
	//set flag if needed, and set the right goal position if user
	// modify it
	if(mdv->goal_speed > 0){
	mdv->goal_pos = mdv->pos_limit;
	if(mdv->moving_pos < mdv->goal_pos ){
	mdv_set_flag(mdv,MDV_F_MOVING_ENABLE);
	}
	} else if (mdv->goal_speed < 0){
	mdv->goal_pos = 0;
	if(mdv->moving_pos > 0 ){
	mdv_set_flag(mdv,MDV_F_MOVING_ENABLE);
	}
	}

	mdv_clear_flag(mdv, MDV_F_NEW_SPEED_AVAILABLE);
	}

	mdv_speed_pid_with_compliance_process(mdv);
	}

	void mdv_velocity_init(motordriver * mdv){
	gpio_set(mdv->pins.coast);
	gpio_set(mdv->pins.brake);
	}


	void mdv_smooth_position_process(motordriver * mdv){
	/// \todo implement safety where if no data is available for an
	/// amount of time, we switch to COAST mode

	if(mdv_flag(mdv,MDV_F_NEW_POSITION_AVAILABLE))
	{
	pushIntoBuffer(&mdv->commandsBuffer, mdv->user_goal_pos);
	}

	// starts the logging of commands whend half the capacity is reached.
	// can be changed to 3/4 or any other fraction of the capacity.
	// this fraction can also be precomputed for more efficiency
	if(!mdv->startSendOrders && mdv->commandsBuffer.numVals >= BUFFER_CAPACITY/2)
	{
	mdv->startSendOrders = 1;
	}

	if(!mdv->startSendOrders)
	{
	mdv_speed_pid_with_compliance_process(mdv);
	return;
	}

	if(mdv->timeCounter < mdv->params.smooth_interval)
	{
	++(mdv->timeCounter);
	mdv_speed_pid_with_compliance_process(mdv);
	return;
	}

	//actually sends the orders if the buffer is not empty.
	if(mdv->commandsBuffer.numVals > 0)
	{
	//gets the goal position from the buffer.
	mdv->goal_pos = mdv_e2i_position(mdv,popFromBuffer(&mdv->commandsBuffer));

	// compute new desired speed, clamp within the range of integers
	long new_speed = clamp_absolute(mdv_lo2hi_res_speed(mdv->goal_pos - mdv->moving_pos) / mdv->params.smooth_interval ,
	mdv->max_speed );

	mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE);
	mdv->goal_speed = new_speed;

	mdv->goal_pos = mdv->moving_pos + (mdv->goal_pos - mdv->moving_pos)*(long)(SMOOTH_POSITION_OVERSHOOT + 1 );

	// limit position
	mdv->goal_pos = clamp(mdv->goal_pos,0,mdv->pos_limit);

	// update goal position
	if (mdv->goal_pos != mdv->moving_pos) {
	// enable new movement
	mdv_set_flag(mdv, MDV_F_MOVING_ENABLE);
	}

	mdv_clear_flag(mdv,MDV_F_NEW_POSITION_AVAILABLE);
	}
	mdv->timeCounter = 0;
	mdv_speed_pid_with_compliance_process(mdv);
	}


	void mdv_smooth_position_init(motordriver * mdv){
	gpio_set(mdv->pins.coast);
	gpio_set(mdv->pins.brake);

	initBuffer(&mdv->commandsBuffer);
	mdv->timeCounter = 0;
	mdv->startSendOrders = 0;
	}


	#define MDV_RUNTIME_CALIBRATION_SUCCESSFUL 0xffff


	#define mdv_rc_in_calibration_region(min,max,pos) ( (min) < (max) ? ( ( (min) <= (pos) ) && ( (pos) <= (max) ) ) : ( ( (pos) <= (max) ) \|\| ( (min) <= (pos) ) ) )

	/**
	* returns 0xffff if calibration is over, otherwise a 12 bits unsigned
	* int being the real position.
	*/

	int mdv_rc_check_and_calibrate(motordriver * mdv){

	/* readout the real position */
	int pos = (*(mdv->calib.pos_evaluator))();


	if(mdv_rc_in_calibration_region(mdv->calib.min_region,mdv->calib.max_region,pos)){

	//we check how much far away we are from the min position, in increments, and we map it to
	//the 0 position, and add the offset.
	long actual_position = (long)pos - (long)mdv->calib.min_region + (long)mdv->calib.offset;
	if(actual_position < 0 ){
	actual_position += mdv->calib.pos_evaluator_max_value;
	}
	actual_position = mdv_e2i_position(mdv,actual_position);

	mdv->pos_limit = mdv_e2i_position(mdv,mdv->calib.pos_limit);

	mdv_set_pos_count(mdv,actual_position);

	return MDV_RUNTIME_CALIBRATION_SUCCESSFUL;
	}

	return pos;

	}

	void mdv_runtime_calibration_init(motordriver * mdv){
	gpio_set(mdv->pins.brake);
	gpio_clear(mdv->pins.coast);

	//this sets an update of 10 ms, and make sure that the next process will
	//compute an update.
	mdv->calib.update_period = 10;
	mdv->calib.ellapsed_ts = 10;
	}


	void mdv_runtime_calibration_process(motordriver * mdv){

	//first we check if we can calibrate
	int pos = mdv_rc_check_and_calibrate(mdv);

	if (pos == MDV_RUNTIME_CALIBRATION_SUCCESSFUL ){
	//yes, and calibrated , so go back to COAST
	mdv->sc.status \|= MDV_RUNTIME_CALIBRATED;
	mdv_set_control_mode_private(mdv,MDV_M_COAST);
	return;
	}

	//now check if we need an update of the target (every mdv->calib.update_period)
	if( (++(mdv->calib.ellapsed_ts)) <= mdv->calib.update_period ) {
	//no update needed, just process PID and return
	mdv_speed_pid_with_compliance_process(mdv);
	return;
	}

	//we need to compute a new update update.

	mdv->calib.ellapsed_ts = 0;

	int mini = mdv->calib.min_region;
	int maxi = mdv->calib.max_region;

	long target_position;

	/* first put the actual absolute error in target position */
	// the boundary are clean
	if (mini < maxi){
	target_position = (mini + maxi) / 2 - pos ;
	} else { //max has an overshoot, we should take it into account
	target_position = (mini + maxi + mdv->calib.pos_evaluator_max_value + 1) / 2 - pos;
	}

	//check for the shortest path !!!
	if(abs(target_position) > ANGULAR_RESOLUTION / 2){
	//we should make more than half a turn, so we go in the other direction !!!
	//case target > 0 remove ANGULAR_RESOLUTION
	//case target < 0 add ANGULAR_RESOLUTION
	target_position += ANGULAR_RESOLUTION * ((target_position > 0) ? -1 : 1);
	}

	//don't forget to put this in the right dimension
	target_position = mdv_e2i_position(mdv,target_position);

	//IGNORE POSITION_LIMIT !!!!!

	//we now now our next target
	mdv->goal_pos = target_position + mdv->pos_cnt ;

	if(mdv->goal_pos != mdv->pos_cnt){
	mdv_set_flag(mdv,MDV_F_MOVING_ENABLE);
	} else {
	mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE);
	}


	mdv->goal_speed = mdv_lo2hi_res_speed((mdv->goal_pos - mdv->pos_cnt) / mdv->calib.update_period);

	//prevent the speed to exceed 2 turn per minute of the effector i.e. ANGULAR_RESOLUTION / 30.
	long max_speed = mdv_e2i_speed(mdv, ANGULAR_RESOLUTION / 30);
	mdv->goal_speed = clamp_absolute(mdv->goal_speed,max_speed);


	//now process the speed pid
	mdv_speed_pid_with_compliance_process(mdv);
	//activate move now
	gpio_set(mdv->pins.coast);

	}

	void mdv_sine_generator_init(motordriver * mdv){
	mdv->sine.t = COS_TABLE_SIZE / 4 - 1;
	mdv->sine.t_residue = 0;
	mdv->sine.iter = 4;
	mdv_set_smooth_interval(mdv,5);
	mdv_smooth_position_init(mdv);
	}


	void mdv_sine_generator_process(motordriver * mdv){
	sbcp_reg_address r = mdv == mdv1 ? MDV_SINE_1 : MDV_SINE_2;
	int16_t frequency = sbcp_reg_table[r + SINE_FREQUENCY].i;
	mdv->sine.t += frequency / 100;
	mdv->sine.t_residue += frequency % 100;
	if(mdv->sine.t_residue > 100){
	mdv->sine.t += 1;
	mdv->sine.t_residue -= 100;
	}

	if(mdv->sine.t > COS_TABLE_SIZE){
	mdv->sine.t -= COS_TABLE_SIZE;
	}

	mdv->sine.iter += 1;

	//seems strange but we clear the flag, only us could move the motor
	//if we need.
	mdv_clear_flag(mdv,MDV_F_NEW_POSITION_AVAILABLE);

	if(mdv->sine.iter % 5 == 0){
	mdv->sine.iter = 0;
	int16_t amp = sbcp_reg_table[r + SINE_AMPLITUDE].i;
	int16_t offset = sbcp_reg_table[r + SINE_OFFSET].i;
	long pos = (long)amp * (long)cos_table[mdv->sine.t] / (long)2048 + (long)offset ;
	mdv_set_goal_position(mdv, pos & 0xffff);
	}
	mdv_smooth_position_process(mdv);
	}

mdv_control_modes.cNo OneTemporaryActions

File Metadata

mdv_control_modes.cView Options

Event Timeline

mdv_control_modes.c
No OneTemporary
Actions

mdv_control_modes.c
View Options