Lab 3 - Virtual Environment Code (Virtual Spring)

////////////////////////////////////////////////////////////////////////
// Declare Pin Assignments
////////////////////////////////////////////////////////////////////////

const int currentSensorPin = 1;
const int positionSensorPin = 7;
const int dirMotorPin = 22;
const int pwmMotorPin = 9;
const int ledPin = 13;
const int speakerPin = 10;

////////////////////////////////////////////////////////////////////////
// Declare Variables
////////////////////////////////////////////////////////////////////////

int sensorValue = 0; // the sensor value
int sensorMin = 1023; // minimum sensor value [Position Sensor]
int sensorMax = 0; // maximum sensor value [Position Sensor]

int t_step = 5; // Delay Between Control Loops (microseconds)
int freq = 2000; // PWM Frequency (Hz)

double thetaMin = -18.0; // Minimum Position Angle
double thetaMax = 18.0; // Maximum Position Angle
double thetaCenter = 0.0; // Center Position Angle
double K = 5;

int position_sign;
double pwmMin = 0.00; // Minimum Duty Cycle
double pwmMax = 1.00; // Maximum Duty Cycle
double A;
double B;

double dt = 50;

double K_m = 0.0153261; // Motor Torque Constant

////////////////////////////////////////////////////////////////////////
// Spring Force Calculation Function
////////////////////////////////////////////////////////////////////////

double torqueSpring(double theta, double thetaCenter, double K ) {

double torque;
double deadZone = 2;

// Below Center Angle
if (theta < (thetaCenter - deadZone)) { torque = (thetaCenter - deadZone - theta)*K; } // Above Center Angle else if (theta > (thetaCenter + deadZone)) {
torque = (thetaCenter + deadZone - theta)*K;
}

// Dead Zone
else { torque = 0; }

return torque;

}

////////////////////////////////////////////////////////////////////////
// Setup Function
////////////////////////////////////////////////////////////////////////

void setup() {

// Set Input Pins

pinMode(currentSensorPin, INPUT);
pinMode(positionSensorPin, INPUT);

// Set Output Pins

pinMode(dirMotorPin, OUTPUT);
pinMode(pwmMotorPin, OUTPUT);
pinMode(ledPin, OUTPUT);
pinMode(speakerPin, OUTPUT);

// Signal Beginning of Calibration
digitalWrite(ledPin, HIGH);

// Calibrate during First 10 Seconds
while (millis() < sensorvalue =" analogRead(positionSensorPin);"> sensorMax) {sensorMax = sensorValue;}

// record the minimum sensor value
if (sensorValue < sensorMin) {sensorMin = sensorValue;}

}

// Signal End of Calibration
digitalWrite(ledPin, LOW);

// Initialize Serial Communication Connection
Serial.begin(9600);

}

////////////////////////////////////////////////////////////////////////
// Loop Function
////////////////////////////////////////////////////////////////////////

void loop() {

double theta;
double dutyCycle = pwmMin;
double current;
double currentDesired;
double torqueDesired;

// Read Position Sensor
sensorValue = analogRead(positionSensorPin);

// Find Angle Using Sensor Value
A = (thetaMax-thetaMin)/(sensorMax-sensorMin);
B = (-0.5*(thetaMax-thetaMin))-(sensorMin*A);

theta = (A*double(sensorValue))+B;

// Print Processed Value to Serial Terminal
// Serial.print(" Theta: ");
// Serial.println(theta);

// Store Direction Information
if (theta < 0) { position_sign = 1; theta = abs(theta); }
else { position_sign = 0; }

// Read Current Sensor
sensorValue = analogRead(currentSensorPin);

// Find Current Using Sensor Value
current = double(sensorValue)*(5/1023);

// Calculate Desired Torque
torqueDesired = torqueSpring(theta, thetaCenter, K );

// Calculate Desired Current
currentDesired = abs(torqueDesired/K_m);

// Print Current
// Serial.print(" Current: ");
// Serial.println(current);
// Serial.print(" Current Desired: ");
// Serial.println(currentDesired);

// Calculate Duty Cycle
double GAIN = 0.01;
dutyCycle = dutyCycle + GAIN*(currentDesired - current);

// Use Limits on Duty Cycle
dutyCycle = constrain(dutyCycle, pwmMin, pwmMax);

// Print dutyCycle to Serial Terminal
// Serial.println(dutyCycle);

// Set H-Bridge Direction
if (position_sign == 0) { digitalWrite(dirMotorPin, LOW); }
else { digitalWrite(dirMotorPin, HIGH); }

// Apply H-Bridge PWM Signal
dutyCycle = dutyCycle*255;
analogWrite(pwmMotorPin,int(dutyCycle));

}