Lab 3 - Virtual Environment Code (Virtual Damper)

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

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

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

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

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 b = 0.0001;

double theta;
double dutyCycle;
double thetaOld = 0;

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

// 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() {

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

if ((theta-thetaOld) < 0) {position_sign = 1;}
else {position_sign = 0;}

// Calculate Duty Cycle
dutyCycle = b*abs(theta-thetaOld)/(0.05);

thetaOld = theta;

// 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 == 1) { digitalWrite(dirMotorPin, LOW); }
else { digitalWrite(dirMotorPin, HIGH); }

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

delay(50);

}