i've been pretty busy lately and have not got shit done with this lately. so i think the best thing to do is see if it will help others. this is pretty much the code thats running in the videos, i should have a schematic up soon but even thats not really complete because i havent build the current sensing stuff yet.
/ set hall pins
int hallAPin = 4;
int hallBPin = 5;
int hallCPin = 6;
// set motor control pins
int motordirPin = 12;
int motorspeedPin = 11;
//ISR variables
int HallA = 0;
int HallB = 0;
int HallC = 0;
volatile int N = 1;
volatile int Z = 0;
volatile int lastN = 1;
//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors)
//PID controller constants
float KP = 3; //position multiplier (gain)
float KI = 0.04; // Intergral multiplier (gain)
float KD = 13.5; // derivative multiplier (gain)
//track the previous error for the derivitive term, and the sum of the errors for the integral term
int lastError = 0;
int sumError = 0;
//Integral term min/max (random value and not yet tested/verified)
int iMax = 100;
int iMin = 0;
//set analog input
int potPin = 4;
int currentPin = 0;
//
void setup() {
// set hall interrupt
attachInterrupt(0, readHall, HIGH);
// set hall pins to input
pinMode (hallAPin, INPUT);
pinMode (hallBPin, INPUT);
pinMode (hallCPin, INPUT);
// set current refence to 1.1volts
analogReference (DEFAULT);
// set pwm prescaler to 32khz
int prescalerVal = 0x07;
TCCR2B &= ~prescalerVal;
prescalerVal = 1;
TCCR2B |= prescalerVal;
// set motor pins to output
pinMode (motordirPin, OUTPUT);
pinMode (motorspeedPin, OUTPUT);
Serial.begin(19200);
}
void loop() {
int val = analogRead(potPin); // read the potentiometerv alue (0 - 1023)
int T = map(val,0,1023,0,300);// set the target to seek to by mapping the potentiometer to the encoder max count
delay(5); //dont change this unless you slow down the main loop somehow, this is a basic servo rate multiplyer and is nessisary because the hallsensor feed back is such low resolution
//also, i have located the delay here and not in the PID routeen because you may want to enter PID from different points in the main loop, some of them needing less delay then others
pid (T);
//Serial.println (analogRead(currentPin));
Serial.println (Z);
/*
//this is the start of the homing routeen, it dosnt work yet. it uses one of the arduinos analog inputs to sens motor current.
//the idea is to move the servo until it binds up a and motorcurrent becomes greatter then currentmax, at that point it will move the motor the other way until it hits currentmax.
//then if the number of steps moved is =>300, the vgt is clear and the current poistion becomes zero and the servo is able to move relative to the control data.
for (int Homin = false; Homin = true; ){
a = a++
if (a=10) {a=0; T=T++}
delay(5);
pid (T);
if (analogRead(currentpin) > currentmax) {if (Z>290) {homin=true;}}
}
*/
}
void pid (int target){
int error = Z - target; // find the error term of current position - target
// generalized PID formula
//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors)
int ms = KP * error + KD * (error - lastError) +KI * (sumError);// calculate a motor speed for the current conditions
// set the last and sumerrors for next loop iteration
lastError = error;
sumError += error;
//scale the sum for the integral term
if (sumError > iMax){sumError = iMax;} else if (sumError < iMin){sumError = iMin;}
int Direction; //determine the direction to go in since the adafruit controller expects posotive values
if(ms > 0){Direction = HIGH;}
if(ms < 0){Direction = LOW; ms = -1 * ms;}
// map the result to the max speed the controller will expect
//(not sure if this is a good idea)
if (ms > 50){ms = 50;}
int Mspeed = map(ms,0,50,85,255);
// output Mspeed and direction
digitalWrite(motordirPin, Direction);
analogWrite(motorspeedPin, Mspeed);
}
//ISR functions
void readHall(){
HallA = digitalRead(hallAPin);
HallB = digitalRead(hallBPin);
HallC = digitalRead(hallCPin);
if ((HallA == HIGH) && (HallB == LOW) && (HallC == HIGH)) {N = 1;} else {lastN = N;}
if ((lastN == 6) && (N == 1)) {Z = Z + 1;}
if ((lastN == 2) && (N == 1)) {Z = Z - 1;}
if ((HallA == HIGH) && (HallB == LOW) && (HallC == LOW)) {N = 2;} else {lastN = N;}
if ((lastN == 1) && (N == 2)) {Z = Z + 1;}
if ((lastN == 3) && (N == 2)) {Z = Z - 1;}
if ((HallA == HIGH) && (HallB == HIGH) && (HallC == LOW)) {N = 3;} else {lastN = N;}
if ((lastN == 2) && (N == 3)) {Z = Z + 1;}
if ((lastN == 4) && (N == 3)) {Z = Z - 1;}
if ((HallA == LOW) && (HallB == HIGH) && (HallC == LOW)) {N = 4;} else {lastN = N;}
if ((lastN == 3) && (N == 4)) {Z = Z + 1;}
if ((lastN == 5) && (N == 4)) {Z = Z - 1;}
if ((HallA == LOW) && (HallB == HIGH) && (HallC == HIGH)) {N = 5;} else {lastN = N;}
if ((lastN == 4) && (N == 5)) {Z = Z + 1;}
if ((lastN == 6) && (N == 5)) {Z = Z - 1;}
if ((HallA == LOW) && (HallB == LOW) && (HallC == HIGH)) {N = 6;} else {lastN = N;}
if ((lastN == 5) && (N == 6)) {Z = Z + 1;}
if ((lastN == 1) && (N == 6)) {Z = Z - 1;}
if ((HallA == HIGH) && (HallB == LOW) && (HallC == HIGH)) {N = 1;} else {lastN = N;}
if ((lastN == 6) && (N == 1)) {Z = Z + 1;}
if ((lastN == 2) && (N == 1)) {Z = Z - 1;}
}
Author
Topic: holset he351 knows were its at (Read 34095 times)