Startup Wizard – ESCON Module 50/5

Motor Data (EC 45 flat Ø42.8 mm, brushless, 70 Watt, with Hall sensors) Part No. 402687
#include "Arduino.h"
// Motor control ports
int enable = 52;
int dir = 53;
int motor = 4;
// Defining Variables
float Encod = A7;
float readvalue;
float Input;
float Volt;
double Setpoint;
float Error;
float PID_OUT = 0;
void setup() {
 pinMode(Encod,INPUT);
pinMode(enable, OUTPUT); // Enable
pinMode(dir, OUTPUT); // Direction *********Creating Error
pinMode(motor, OUTPUT); // PWM
digitalWrite(enable, HIGH);
Serial.begin(115200);
Setpoint = 20;
Setpoint = (Setpoint*3.1416)/180;
}

void loop() {
analogReadResolution(10);
while (1) {
//....................................... Data Mapping.............................................
//::::::::::::::::::::::::::::::::::ENCODER = VOLTS = ANGLE::::::::::::::::::::::::::::::::::::::::
//:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
readvalue = analogRead(Encod); //Read the Min = 146 and Max = 516 Values from ENCODER
// Serial.print("Encoder");
//Serial.println(readvalue);
Volt = mapfloat(readvalue, 146,516,0.00,3.30); // Mapping the float from 146 = 0 and 516 = 3.30
//Serial.print("Volt");
//Serial.println(Volt);
Input = mapfloat(Volt, 0.00,3.30,10,120); // Mapping the float from 0 = 10 (Flexion) and 3.30 = 120
(Extension);
//Serial.print("Input");
//Serial.println(Input);
Input = (Input*3.1416)/180;
// ::::::::::::::::::::::::::::::::::::::::Error Define::::::::::::::::::::::::::::::::::::::::::::
Error = Setpoint - Input;
PID_OUT = 0.05*Error;

if (PID_OUT >= 0)
{
PID_OUT = PID_OUT;
digitalWrite(dir, HIGH);
// Serial.println("LOW");
}
if (PID_OUT < 0)
{
PID_OUT = -1 * PID_OUT;
digitalWrite(dir, LOW);
// Serial.println("HI");
}
if (PID_OUT > 2)
{
PID_OUT = 2;
}
//PID_OUT = 1.0923e+3*PID_OUT + 409.6;
//PID_OUT = 273.0667*PID_OUT + 102.4;
 //PID_OUT = -0.1667*PID_OUT + 25.5;
PID_OUT = mapfloat(PID_OUT, 0.00,2.00,102.4,1024); //Map 0 at 10
analogWrite(motor,PID_OUT);
//Serial.print("Read");
//Serial.println(Input*180/3.14);
//Serial.print("Setpoint");
//Serial.println(Setpoint*180/3.14);

//Serial.print("Error");
//Serial.println(Error);

//delay(500);
}
}
float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

Running The Velocity Control

Shoulder Extension/Flexion Motor