Skip to content

Instantly share code, notes, and snippets.

@jpearman
Last active February 4, 2018 09:06
Show Gist options
  • Save jpearman/df388db22e169c6cb422 to your computer and use it in GitHub Desktop.
Save jpearman/df388db22e169c6cb422 to your computer and use it in GitHub Desktop.
Simplified version of flywheel control code that does not use a structure to hold all variables
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1, , sensorQuadEncoderOnI2CPort, , AutoAssign)
#pragma config(Motor, port2, Motor_FW1, tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor, port3, Motor_FW2, tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor, port4, Motor_FW3, tmotorVex393HighSpeed_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
/*-----------------------------------------------------------------------------*/
/* */
/* Copyright (c) James Pearman */
/* 2015 */
/* All Rights Reserved */
/* */
/*-----------------------------------------------------------------------------*/
/* */
/* Module: flywheel_s.c */
/* Author: James Pearman */
/* Created: 28 June 2015 */
/* */
/* Revisions: */
/* V1.00 28 June 2015 - Initial release */
/* */
/*-----------------------------------------------------------------------------*/
/* */
/* The author is supplying this software for use with the VEX cortex */
/* control system. This file can be freely distributed and teams are */
/* authorized to freely use this program , however, it is requested that */
/* improvements or additions be shared with the Vex community via the vex */
/* forum. Please acknowledge the work of the authors when appropriate. */
/* Thanks. */
/* */
/* Licensed under the Apache License, Version 2.0 (the "License"); */
/* you may not use this file except in compliance with the License. */
/* You may obtain a copy of the License at */
/* */
/* http://www.apache.org/licenses/LICENSE-2.0 */
/* */
/* Unless required by applicable law or agreed to in writing, software */
/* distributed under the License is distributed on an "AS IS" BASIS, */
/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. */
/* See the License for the specific language governing permissions and */
/* limitations under the License. */
/* */
/* The author can be contacted on the vex forums as jpearman */
/* or electronic mail using jbpearman_at_mac_dot_com */
/* Mentor for team 8888 RoboLancers, Pasadena CA. */
/* */
/*-----------------------------------------------------------------------------*/
/* */
/* An example of flywheel/shooter velocity control using the TBH algorithm */
/* Test system uses three motors with 25:2 gearing to the flywheel. */
/* */
/* A modofied version of the original that does not use a structure to */
/* keep all related variables together. */
/*-----------------------------------------------------------------------------*/
// Update inteval (in mS) for the flywheel control loop
#define FW_LOOP_SPEED 25
// Maximum power we want to send to the flywheel motors
#define FW_MAX_POWER 127
// encoder counts per revolution depending on motor
#define MOTOR_TPR_269 240.448
#define MOTOR_TPR_393R 261.333
#define MOTOR_TPR_393S 392
#define MOTOR_TPR_393T 627.2
#define MOTOR_TPR_QUAD 360.0
// encoder tick per revolution
float ticks_per_rev; ///< encoder ticks per revolution
// Encoder
long encoder_counts; ///< current encoder count
long encoder_counts_last; ///< current encoder count
// velocity measurement
float motor_velocity; ///< current velocity in rpm
long nSysTime_last; ///< Time of last velocity calculation
// TBH control algorithm variables
long target_velocity; ///< target_velocity velocity
float current_error; ///< error between actual and target_velocity velocities
float last_error; ///< error last time update called
float gain; ///< gain
float drive; ///< final drive out of TBH (0.0 to 1.0)
float drive_at_zero; ///< drive at last zero crossing
long first_cross; ///< flag indicating first zero crossing
float drive_approx; ///< estimated open loop drive
// final motor drive
long motor_drive; ///< final motor control value
/*-----------------------------------------------------------------------------*/
/** @brief Set the flywheen motors */
/** @param[in] value motor control value */
/*-----------------------------------------------------------------------------*/
void
FwMotorSet( int value )
{
motor[ Motor_FW1 ] = value;
motor[ Motor_FW2 ] = value;
motor[ Motor_FW3 ] = value;
}
/*-----------------------------------------------------------------------------*/
/** @brief Get the flywheen motor encoder count */
/*-----------------------------------------------------------------------------*/
long
FwMotorEncoderGet()
{
return( nMotorEncoder[ Motor_FW1 ] );
}
/*-----------------------------------------------------------------------------*/
/** @brief Set the controller position */
/** @param[in] desired velocity */
/** @param[in] predicted_drive estimated open loop motor drive */
/*-----------------------------------------------------------------------------*/
void
FwVelocitySet( int velocity, float predicted_drive )
{
// set target_velocity velocity (motor rpm)
target_velocity = velocity;
// Set error so zero crossing is correctly detected
current_error = target_velocity - motor_velocity;
last_error = current_error;
// Set predicted open loop drive value
drive_approx = predicted_drive;
// Set flag to detect first zero crossing
first_cross = 1;
// clear tbh variable
drive_at_zero = 0;
}
/*-----------------------------------------------------------------------------*/
/** @brief Calculate the current flywheel motor velocity */
/*-----------------------------------------------------------------------------*/
void
FwCalculateSpeed()
{
int delta_ms;
int delta_enc;
// Get current encoder value
encoder_counts = FwMotorEncoderGet();
// This is just used so we don't need to know how often we are called
// how many mS since we were last here
delta_ms = nSysTime - nSysTime_last;
nSysTime_last = nSysTime;
// Change in encoder count
delta_enc = (encoder_counts - encoder_counts_last);
// save last position
encoder_counts_last = encoder_counts;
// Calculate velocity in rpm
motor_velocity = (1000.0 / delta_ms) * delta_enc * 60.0 / ticks_per_rev;
}
/*-----------------------------------------------------------------------------*/
/** @brief Update the velocity tbh controller variables */
/*-----------------------------------------------------------------------------*/
void
FwControlUpdateVelocityTbh()
{
// calculate error in velocity
// target_velocity is desired velocity
// current is measured velocity
current_error = target_velocity - motor_velocity;
// Calculate new control value
drive = drive + (current_error * gain);
// Clip to the range 0 - 1.
// We are only going forwards
if( drive > 1 )
drive = 1;
if( drive < 0 )
drive = 0;
// Check for zero crossing
if( sgn(current_error) != sgn(last_error) ) {
// First zero crossing after a new set velocity command
if( first_cross ) {
// Set drive to the open loop approximation
drive = drive_approx;
first_cross = 0;
}
else
drive = 0.5 * ( drive + drive_at_zero );
// Save this drive value in the "tbh" variable
drive_at_zero = drive;
}
// Save last error
last_error = current_error;
}
/*-----------------------------------------------------------------------------*/
/** @brief Task to control the velocity of the flywheel */
/*-----------------------------------------------------------------------------*/
task
FwControlTask()
{
// Set the gain
gain = 0.00025;
// We are using Speed geared motors
// Set the encoder ticks per revolution
ticks_per_rev = MOTOR_TPR_393S;
while(1)
{
// Calculate velocity
FwCalculateSpeed();
// Do the velocity TBH calculations
FwControlUpdateVelocityTbh() ;
// Scale drive into the range the motors need
motor_drive = (drive * FW_MAX_POWER) + 0.5;
// Final Limit of motor values - don't really need this
if( motor_drive > 127 ) motor_drive = 127;
if( motor_drive < -127 ) motor_drive = -127;
// and finally set the motor control value
FwMotorSet( motor_drive );
// Run at somewhere between 20 and 50mS
wait1Msec( FW_LOOP_SPEED );
}
}
// Main user task
task main()
{
char str[32];
bLCDBacklight = true;
// Start the flywheel control task
startTask( FwControlTask );
// Main user control loop
while(1)
{
// Different speeds set by buttons
if( vexRT[ Btn8L ] == 1 )
FwVelocitySet( 144, 0.55 );
if( vexRT[ Btn8U ] == 1 )
FwVelocitySet( 120, 0.38 );
if( vexRT[ Btn8R ] == 1 )
FwVelocitySet( 50, 0.2 );
if( vexRT[ Btn8D ] == 1 )
FwVelocitySet( 00, 0 );
// Display useful things on the LCD
sprintf( str, "%4d %4d %5.2f", target_velocity, motor_velocity, nImmediateBatteryLevel/1000.0 );
displayLCDString(0, 0, str );
sprintf( str, "%4.2f %4.2f ", drive, drive_at_zero );
displayLCDString(1, 0, str );
// Don't hog the cpu :)
wait1Msec(10);
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment