Lab 3 - Virtual Environment Code (Virtual Spring-Mass)

////////////////////////////////////////////////////////////////////////
// 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

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

double K_m = 0.0153261; // Motor Torque Constant
double pi = 3.1415;
double delay_ = 0.001; //delay in seconds

////////////////////////////////////////////////////////////////////////
// VIRTUAL MASS-SPRING SYSTEM
////////////////////////////////////////////////////////////////////////

double xBlock0 = 0.;//0.05;
double xBlock = 0.05;
double vBlock = 0.;
double aBlock = 0.;
double mBlock = 10.;
double rLever = 0.02;
double kSpring = 100.;
double dt = 0.05;

////////////////////////////////////////////////////////////////////////
// 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);

double force;

// Calculate Desired Torque
force = -1*kSpring*(xBlock - rLever*sin(theta*pi/180.));
aBlock = force/mBlock;

vBlock += aBlock*delay_;
xBlock += 0.5*aBlock*(delay_*delay_)+vBlock*delay_;

dutyCycle = force*rLever;

if (dutyCycle < 0) {position_sign = 0;}
else {position_sign = 1;}

// Use Limits on Duty Cycle
dutyCycle = constrain(abs(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));

delay(int(delay_*1000.));
}