Rotork Research Team
/
TFM_Encoder
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: main.cpp
- Revision:
- 16:e6c8df9960c6
- Parent:
- 15:12a4bbfa6de4
- Child:
- 17:19b2c598810a
--- a/main.cpp Fri Jan 25 15:24:07 2019 +0000
+++ b/main.cpp Wed Feb 06 15:46:51 2019 +0000
@@ -2,7 +2,7 @@
#include "QEI.h"
void Initialisation (void); //These voids are written after the main. They must be listed here too (functional prototypes).
-void StepCW(void);
+void StepACW(void);
void Ph0(void);
void Ph1(void);
void Ph12 (void);
@@ -25,10 +25,10 @@
PwmOut Phase1 (p21); //Pin and LED set up - originally standard pins but changed to PWM to enable speed control
PwmOut Phase2 (p22);
-PwmOut Phase3 (p25);
-PwmOut Phase4 (p26);
-DigitalOut UnUsedPhase1 (p23);
-DigitalOut UnUsedPhase2 (p24);
+PwmOut Phase3 (p23);
+PwmOut Phase4 (p24);
+DigitalOut UnUsedPhase1 (p25);
+DigitalOut UnUsedPhase2 (p26);
//AnalogOut Aout(p18); //Used with multimeter to give a speed indicator 1mV = 1RPM
@@ -55,6 +55,12 @@
int rpm = 0; //for calc of RPM
int SetPoint = 1500; //for adjusting the speed
int enc = 3200;
+int i = 0;
+int j = 0;
+int k = 0;
+int l = 0;
+int m = 0;
+int n = 0;
int s = enc/50;
int z = 3200; //TimePerRev = TimePerClick * (800/z); 800 pulses per rev, PulseCount2_==800 for wheel.getwhoop_ flag. i.e. 10 points per reoluition for RPM calc.
int T = 80; //Motor temp limit
@@ -64,7 +70,8 @@
float duty = 1; //velocity loop: 1 = fastest, 0.96 = slowest. Below 0.96 the motor will not operate.
float diff = 0.0; //Velocity loop: diff = SetPoint - rpm;
-float AdjDiff = 0.0001;
+float AdjDiff = 0.00001;
+float p = 0.000014;
float x=0.1; //x=time of square wave when 1 phase energised,
float TimePerRev = 0; //for calc of RPM
float y=0.04; //y=time of square wave when 2 phases energised
@@ -73,14 +80,14 @@
int main(void)
{
pc.baud(230400); //Set fastest baud rate
- Phase1.period(0.00001); //period of 0.000002 = 2 microseconds (500kHz). Good balance of low and high speed performance.
- Phase2.period(0.00001);
- Phase3.period(0.00001);
- Phase4.period(0.00001);
+ Phase1.period(p); //period of 0.000002 = 2 microseconds (500kHz). Good balance of low and high speed performance.
+ Phase2.period(p);
+ Phase3.period(p);
+ Phase4.period(p);
wait(0.1);
t.start();
SerialClock = 0;
- StepCW();
+ StepACW();
Initialisation();
/*while(1)
@@ -176,14 +183,16 @@
while((led1 == 0) && (led2 == 0)) //If no command to operate
{
//Aout = 0;
- //duty = 0.5;
+ //duty = 0.7;
rpm = 0;
AdjCW = CW;
AdjACW = ACW;
Ph0();
GetChar();
ReadKType();
+ //VelocityLoop();
StateB = (wheel.getPulses()+StateA)%s; //wheel.getPulses()%1(s-1);
+ //wait(0.1);
//StateC = (800+wheel.getPulses()+StateA+AdjCW)%s;
//if(wheel.getPulses()==wheel.getPulses()+1);
//{
@@ -195,6 +204,10 @@
while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led1==1) && (temp<T)) //After Calibration, Prev CW movement, CW command
{
GetChar();
+ //Phase1.period(p); //period of 0.000002 = 2 microseconds (500kHz). Good balance of low and high speed performance.
+ //Phase2.period(p);
+ //Phase3.period(p);
+ //Phase4.period(p);
StateB = (wheel.getPulses()+StateA+AdjCW)%s;
//pc.printf("rpm = %i, whoop = %i\n\r", rpm, wheel.getWhoop());
//pc.printf("StateB= %i\n\r", StateB);
@@ -212,12 +225,12 @@
case 7:Ph1();break;
case 8:Ph1();break;
case 9:Ph1();break;
- case 10:Ph1();break;
+ case 10:Ph0();break;
case 11:Ph1();break;
case 12:Ph1();break;
- case 13:Ph1();break;
+ case 13:Ph0();break;
case 14:Ph1();break;
- case 15:Ph1();break;
+ case 15:Ph0();break;
case 16:Ph2();break;
case 17:Ph2();break;
case 18:Ph2();break;
@@ -228,12 +241,12 @@
case 23:Ph2();break;
case 24:Ph2();break;
case 25:Ph2();break;
- case 26:Ph2();break;
+ case 26:Ph0();break;
case 27:Ph2();break;
case 28:Ph2();break;
- case 29:Ph2();break;
+ case 29:Ph0();break;
case 30:Ph2();break;
- case 31:Ph2();break;
+ case 31:Ph0();break;
case 32:Ph3();break;
case 33:Ph3();break;
case 34:Ph3();break;
@@ -244,12 +257,12 @@
case 39:Ph3();break;
case 40:Ph3();break;
case 41:Ph3();break;
- case 42:Ph3();break;
+ case 42:Ph0();break;
case 43:Ph3();break;
case 44:Ph3();break;
- case 45:Ph3();break;
+ case 45:Ph0();break;
case 46:Ph3();break;
- case 47:Ph3();break;
+ case 47:Ph0();break;
case 48:Ph4();break;
case 49:Ph4();break;
case 50:Ph4();break;
@@ -260,27 +273,28 @@
case 55:Ph4();break;
case 56:Ph4();break;
case 57:Ph4();break;
- case 58:Ph4();break;
+ case 58:Ph0();break;
case 59:Ph4();break;
case 60:Ph4();break;
- case 61:Ph4();break;
+ case 61:Ph0();break;
case 62:Ph4();break;
- case 63:Ph4();break;
- default:break;
+ case 63:Ph0();break;
+ default:break;
}
/*if(wheel.getYay()==1)
{
pc.printf("1 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
}*/
- if(wheel.getWhoop()==1) //PulseCount2_==800, whoop_=1;
+ if(wheel.getWhoop()==1) //PulseCount2_==3200, whoop_=1;
{
RPM();
VelocityLoop();
slowloop++;
- if(slowloop>rpm)
+ if(slowloop>(0.01*rpm))
{
ReadKType();
slowloop=0;
+ //pc.printf("%i, %.4f\n\r", rpm, duty);
}
}
}
@@ -288,6 +302,10 @@
while(wheel.getRevolutions()>1 && wheel.getPulses()<1 && (led1==1) && (temp<T)) //After Calibration, Prev ACW movement, CW command
{
GetChar();
+ //Phase1.period(p); //period of 0.000002 = 2 microseconds (500kHz). Good balance of low and high speed performance.
+ //Phase2.period(p);
+ //Phase3.period(p);
+ //Phase4.period(p);
StateB = (enc+wheel.getPulses()+StateA+AdjCW)%s;
//pc.printf("StateA= %i\r", StateA);
//pc.printf("2 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
@@ -304,12 +322,12 @@
case 7:Ph1();break;
case 8:Ph1();break;
case 9:Ph1();break;
- case 10:Ph1();break;
+ case 10:Ph0();break;
case 11:Ph1();break;
case 12:Ph1();break;
- case 13:Ph1();break;
+ case 13:Ph0();break;
case 14:Ph1();break;
- case 15:Ph1();break;
+ case 15:Ph0();break;
case 16:Ph2();break;
case 17:Ph2();break;
case 18:Ph2();break;
@@ -320,12 +338,12 @@
case 23:Ph2();break;
case 24:Ph2();break;
case 25:Ph2();break;
- case 26:Ph2();break;
+ case 26:Ph0();break;
case 27:Ph2();break;
case 28:Ph2();break;
- case 29:Ph2();break;
+ case 29:Ph0();break;
case 30:Ph2();break;
- case 31:Ph2();break;
+ case 31:Ph0();break;
case 32:Ph3();break;
case 33:Ph3();break;
case 34:Ph3();break;
@@ -336,12 +354,12 @@
case 39:Ph3();break;
case 40:Ph3();break;
case 41:Ph3();break;
- case 42:Ph3();break;
+ case 42:Ph0();break;
case 43:Ph3();break;
case 44:Ph3();break;
- case 45:Ph3();break;
+ case 45:Ph0();break;
case 46:Ph3();break;
- case 47:Ph3();break;
+ case 47:Ph0();break;
case 48:Ph4();break;
case 49:Ph4();break;
case 50:Ph4();break;
@@ -352,33 +370,38 @@
case 55:Ph4();break;
case 56:Ph4();break;
case 57:Ph4();break;
- case 58:Ph4();break;
+ case 58:Ph0();break;
case 59:Ph4();break;
case 60:Ph4();break;
- case 61:Ph4();break;
+ case 61:Ph0();break;
case 62:Ph4();break;
- case 63:Ph4();break;
+ case 63:Ph0();break;
default:break;
}
/*if(wheel.getYay()==1)
{
pc.printf("2 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
}*/
- if(wheel.getWhoop()==1) //PulseCount2_==800, whoop_=1;
+ if(wheel.getWhoop()==1) //PulseCount2_==3200, whoop_=1;
{
RPM();
VelocityLoop();
slowloop++;
- if(slowloop>rpm)
+ if(slowloop>(0.01*rpm))
{
ReadKType();
slowloop=0;
+ //pc.printf("%i, %.4f\n\r", rpm, duty);
}
}
}
while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led2==1) && (temp<T)) //After Calibration, Prev CW movement, ACW command
{
GetChar();
+ //Phase1.period(p); //period of 0.000002 = 2 microseconds (500kHz). Good balance of low and high speed performance.
+ //Phase2.period(p);
+ //Phase3.period(p);
+ //Phase4.period(p);
//StateB = (800+wheel.getPulses())%16;
StateB = (wheel.getPulses()+StateA+AdjACW)%s;
//pc.printf("StateA= %i\r", StateA);
@@ -395,12 +418,12 @@
case 56:Ph4();break;
case 55:Ph4();break;
case 54:Ph4();break;
- case 53:Ph4();break;
+ case 53:Ph0();break;
case 52:Ph4();break;
case 51:Ph4();break;
- case 50:Ph4();break;
+ case 50:Ph0();break;
case 49:Ph4();break;
- case 48:Ph4();break;
+ case 48:Ph0();break;
case 47:Ph3();break;
case 46:Ph3();break;
case 45:Ph3();break;
@@ -411,12 +434,12 @@
case 40:Ph3();break;
case 39:Ph3();break;
case 38:Ph3();break;
- case 37:Ph3();break;
+ case 37:Ph0();break;
case 36:Ph3();break;
case 35:Ph3();break;
- case 34:Ph3();break;
+ case 34:Ph0();break;
case 33:Ph3();break;
- case 32:Ph3();break;
+ case 32:Ph0();break;
case 31:Ph2();break;
case 30:Ph2();break;
case 29:Ph2();break;
@@ -427,12 +450,12 @@
case 24:Ph2();break;
case 23:Ph2();break;
case 22:Ph2();break;
- case 21:Ph2();break;
+ case 21:Ph0();break;
case 20:Ph2();break;
case 19:Ph2();break;
- case 18:Ph2();break;
+ case 18:Ph0();break;
case 17:Ph2();break;
- case 16:Ph2();break;
+ case 16:Ph0();break;
case 15:Ph1();break;
case 14:Ph1();break;
case 13:Ph1();break;
@@ -443,33 +466,38 @@
case 8:Ph1();break;
case 7:Ph1();break;
case 6:Ph1();break;
- case 5:Ph1();break;
+ case 5:Ph0();break;
case 4:Ph1();break;
case 3:Ph1();break;
- case 2:Ph1();break;
+ case 2:Ph0();break;
case 1:Ph1();break;
- case 0:Ph1();break;
+ case 0:Ph0();break;
default:break;
}
/*if(wheel.getYay()==1)
{
pc.printf("3 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
}*/
- if(wheel.getWhoop()==1) //PulseCount2_==800, whoop_=1;
+ if(wheel.getWhoop()==1) //PulseCount2_==3200, whoop_=1;
{
RPM();
VelocityLoop();
slowloop++;
- if(slowloop>rpm)
+ if(slowloop>(0.01*rpm))
{
ReadKType();
slowloop=0;
+ //pc.printf("%i, %.4f\n\r", rpm, duty);
}
}
}
while((wheel.getRevolutions()>1) && (wheel.getPulses()<1) && (led2==1) &&(temp<T)) //After Calibration, Prev ACW movement, ACW command
{
GetChar();
+ //Phase1.period(p); //period of 0.000002 = 2 microseconds (500kHz). Good balance of low and high speed performance.
+ //Phase2.period(p);
+ //Phase3.period(p);
+ //Phase4.period(p);
StateB = (enc+wheel.getPulses()+StateA+AdjACW)%s;
//pc.printf("StateA= %i\r", StateA);
//pc.printf("4 StateB= %i, Pulses= %i, Revs= %i \n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
@@ -485,12 +513,12 @@
case 56:Ph4();break;
case 55:Ph4();break;
case 54:Ph4();break;
- case 53:Ph4();break;
+ case 53:Ph0();break;
case 52:Ph4();break;
case 51:Ph4();break;
- case 50:Ph4();break;
+ case 50:Ph0();break;
case 49:Ph4();break;
- case 48:Ph4();break;
+ case 48:Ph0();break;
case 47:Ph3();break;
case 46:Ph3();break;
case 45:Ph3();break;
@@ -501,12 +529,12 @@
case 40:Ph3();break;
case 39:Ph3();break;
case 38:Ph3();break;
- case 37:Ph3();break;
+ case 37:Ph0();break;
case 36:Ph3();break;
case 35:Ph3();break;
- case 34:Ph3();break;
+ case 34:Ph0();break;
case 33:Ph3();break;
- case 32:Ph3();break;
+ case 32:Ph0();break;
case 31:Ph2();break;
case 30:Ph2();break;
case 29:Ph2();break;
@@ -517,12 +545,12 @@
case 24:Ph2();break;
case 23:Ph2();break;
case 22:Ph2();break;
- case 21:Ph2();break;
+ case 21:Ph0();break;
case 20:Ph2();break;
case 19:Ph2();break;
- case 18:Ph2();break;
+ case 18:Ph0();break;
case 17:Ph2();break;
- case 16:Ph2();break;
+ case 16:Ph0();break;
case 15:Ph1();break;
case 14:Ph1();break;
case 13:Ph1();break;
@@ -533,27 +561,28 @@
case 8:Ph1();break;
case 7:Ph1();break;
case 6:Ph1();break;
- case 5:Ph1();break;
+ case 5:Ph0();break;
case 4:Ph1();break;
case 3:Ph1();break;
- case 2:Ph1();break;
+ case 2:Ph0();break;
case 1:Ph1();break;
- case 0:Ph1();break;
+ case 0:Ph0();break;
default:break;
}
/*if(wheel.getYay()==1)
{
pc.printf("4 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
}*/
- if(wheel.getWhoop()==1) //PulseCount2_==800, whoop_=1;
+ if(wheel.getWhoop()==1) //PulseCount2_==3200, whoop_=1;
{
RPM();
VelocityLoop();
slowloop++;
- if(slowloop>rpm)
+ if(slowloop>(0.01*rpm))
{
ReadKType();
slowloop=0;
+ //pc.printf("%i, %.4f\n\r", rpm, duty);
}
}
}
@@ -575,33 +604,8 @@
}
}
}
-void StepCW(void) //Square wave switching
+void StepACW(void) //Square wave switching
{
- /*Ph1();
- wait(x);
- //pc.printf("1 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
- Ph12();
- wait(y);
- //pc.printf("12 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
- Ph2();
- wait(x);
- //pc.printf("2 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
- Ph23();
- wait(y);
- //pc.printf("23 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
- Ph3();
- wait(x);
- //pc.printf("3 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
- Ph34();
- wait(y);
- //pc.printf("34 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
- Ph4();
- wait(x);
- //pc.printf("4 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
- Ph41();
- wait(y);
- //pc.printf("41 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
- */
Ph4();
wait(x);
//pc.printf("4 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
@@ -733,25 +737,24 @@
{ if (pc.readable())
{
c = pc.getc();
+ if(c ==' ')
+ {
+pc.printf(" 0 = Phase Mapping\n\rq = setpoint+, w = duty+, e = AdjDiff+, t = temp, y = states, o = AdjCW+, p = AdjACW+,\n\r a = setpoint-, s = duty-, d = AdjDiff-, k = AdjCW-, l = AdjACW-,\n\r z = CW, x = ACW\n\r");
+ }
if(c == 'z') //turn on led1 causes CW operation
{
- //AdjCW = 0;
- //AdjACW = 10;
led1 = !led1;
led2 = 0;
pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
}
if(c == 'x') //turn on led2 causes ACW operation
{
- //AdjCW = 0;
- //AdjACW = 10;
led1 = 0;
led2 = !led2 ;
pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
}
if(c == 'q') //Increases setpoint used in Velocity loop
{
- //duty = duty + 0.0001;
SetPoint=SetPoint+5;
if (SetPoint >3000)
{
@@ -761,7 +764,6 @@
}
if(c == 'a') //Decreases setpoint used in Velocity loop
{
- //duty = duty - 0.0001;
SetPoint=SetPoint-5;
if (SetPoint <50)
{
@@ -771,43 +773,23 @@
}
if(c == 'w') //Increases setpoint used in Velocity loop
{
- duty = duty + 0.1;
- /*SetPoint=SetPoint+5;
- if (SetPoint >2200)
- {
- SetPoint = 2200;
- }*/
- pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
+ duty = duty + 0.01;
+ pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
}
if(c == 's') //Decreases setpoint used in Velocity loop
{
- duty = duty - 0.1;
- /*SetPoint=SetPoint-5;
- if (SetPoint <50)
- {
- SetPoint = 50;
- }*/
- pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
+ duty = duty - 0.01;
+ pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
}
if(c == 'e') //Increases setpoint used in Velocity loop
{
AdjDiff = AdjDiff * 10;
- /*SetPoint=SetPoint+5;
- if (SetPoint >2200)
- {
- SetPoint = 2200;
- }*/
- pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
+ pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
}
if(c == 'd') //Decreases setpoint used in Velocity loop
{
AdjDiff = AdjDiff / 10;
- /*SetPoint=SetPoint-5;
- if (SetPoint <50)
- {
- SetPoint = 50;
- }*/
- pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
+ pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
}
if(c== 'o')
{
@@ -845,9 +827,37 @@
}
pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
}
+ if(c== 'i')
+ {
+ n = n+1;
+ /*if (n > 63)
+ {
+ n = 0;
+ }*/
+ pc.printf("%i, %.5f, %i, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, n);
+ }
+ if(c== 'j')
+ {
+ n = n-1;
+ /*if (n <0)
+ {
+ n = 63;
+ }*/
+ pc.printf("%i, %.5f, %i, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, n);
+ }
+ if(c== 'r')
+ {
+ p = p+0.0000001;
+ pc.printf("%i, %.5f, %i, %i, %i, %0.7f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, p);
+ }
+ if(c== 'f')
+ {
+ p = p-0.0000001;
+ pc.printf("%i, %.5f, %i, %i, %i, %0.7f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, p);
+ }
if(c=='0')
{
- pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
+ pc.printf("%i, %.5f, %i, %i, %i, %i, %f, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, n, AdjDiff, p);
}
if(c=='t')
{
@@ -855,7 +865,7 @@
}
if(c=='y')
{
- pc.printf("3 StateB= %i, Pulses= %i, Revs= %i \n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
+ pc.printf("State A = %i, StateB = %i, Pulses= %i, Revs= %i \n\r", StateA, StateB, wheel.getPulses(), wheel.getRevolutions());
}
}
}
@@ -871,101 +881,666 @@
rpm = RPS * 60; //x 60 to convert RPS to RPM
//Aout=((0.30303*rpm)/1000); // for 500 rpm (0.30303*500/1000)*3.3V = 0.500V
//ReadKType();
- if(rpm < 300)
+ if(duty < 0.7)
{
- AdjCW = 57;
- AdjACW = 12;
- }
- if(rpm > 299 && rpm <400)
- {
- AdjCW = 62;
- AdjACW = 7;
+ if(rpm < 65)
+ {
+ AdjCW = (57);
+ AdjACW = (12);
+ }
+ if(rpm > 64 and rpm <75)
+ {
+ AdjCW = (57);
+ AdjACW = (12);
+ }
+ if(rpm > 74 and rpm < 90)
+ {
+ AdjCW = (60);
+ AdjACW = (9);
+ }
+ if(rpm > 89 and rpm < 200)
+ {
+ AdjCW = (60);
+ AdjACW = (9);
+ }
+ if(rpm > 199 and rpm < 300)
+ {
+ AdjCW = (61);
+ AdjACW = (8);
+ }
+ if(rpm > 299 and rpm <400)
+ {
+ AdjCW = (62);
+ AdjACW = (7);
+ }
+ if(rpm > 399 and rpm < 525)
+ {
+ AdjCW = (63);
+ AdjACW = (6);
+ }
+ if(rpm > 524 and rpm < 625)
+ {
+ AdjCW = (0);
+ AdjACW = (5);
+ }
+ if(rpm > 624 and rpm < 725)
+ {
+ AdjCW = (1);
+ AdjACW = (4);
+ }
+ if(rpm > 724 and rpm < 850)
+ {
+ AdjCW = (2);
+ AdjACW = (3);
+ }
+ if(rpm > 849 and rpm < 975)
+ {
+ AdjCW = (3);
+ AdjACW = (2);
+ }
+ if(rpm > 974 and rpm < 1150)
+ {
+ AdjCW = (4);
+ AdjACW = (1-n);
+ }
+ if(rpm > 1149 and rpm < 1375)
+ {
+ AdjCW = (5);
+ AdjACW = (0);
+ }
+ if(rpm > 1374 and rpm < 1725)
+ {
+ AdjCW = (6);
+ AdjACW = (63);
+ }
+ if(rpm > 1724 and rpm < 2150)
+ {
+ AdjCW = (7);
+ AdjACW = (62);
+ }
+ if(rpm > 2149 and rpm < 2525)
+ {
+ AdjCW = (8);
+ AdjACW = (61);
+ }
+ if(rpm > 2524 and rpm < 2600)
+ {
+ AdjCW = (9);
+ AdjACW = (60);
+ }
+ if(rpm > 2599 and rpm < 2650)
+ {
+ AdjCW = (10);
+ AdjACW = (59);
+ }
+ if(rpm > 2649 and rpm < 2700)
+ {
+ AdjCW = (11);
+ AdjACW = (58);
+ }
+ if(rpm > 2699 and rpm < 2750)
+ {
+ AdjCW = (12);
+ AdjACW = (57);
+ }
+ if(rpm > 2749 and rpm < 2825)
+ {
+ AdjCW = (13);
+ AdjACW = (56);
+ }
+ if(rpm > 2824 and rpm < 2875)
+ {
+ AdjCW = (14);
+ AdjACW = (55);
+ }
+ if(rpm > 2874)
+ {
+ AdjCW = (15);
+ AdjACW = (54);
+ }
}
- if(rpm > 399 and rpm < 525)
+ if(duty > 0.69)
{
- AdjCW = 63;
- AdjACW = 6;
+ if(rpm < 175)
+ {
+ AdjCW = (60);
+ AdjACW = (11);
+ }
+ if(rpm > 200 and rpm < 275)
+ {
+ AdjCW = (5);
+ AdjACW = (11);
+ }if(rpm > 300 and rpm < 1000)
+ {
+ AdjCW = (5);
+ AdjACW = (5);
+ }
}
- if(rpm > 524 and rpm < 625)
+ /*if(rpm < 180)
{
- AdjCW = 0;
- AdjACW = 5;
+ AdjCW = (57+n);
+ AdjACW = (12-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
}
- if(rpm > 624 and rpm < 725)
+ if(rpm > 179 and rpm <195)
{
- AdjCW = 1;
- AdjACW = 4;
+ AdjCW = (58+n);
+ AdjACW = (11-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
}
- if(rpm > 724 and rpm < 850)
+ if(rpm > 194 and rpm <220)
+ {
+ AdjCW = (59+n);
+ AdjACW = (10-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
+ }
+ if(rpm > 219 and rpm <240)
{
- AdjCW = 2;
- AdjACW = 3;
+ AdjCW = (60+n);
+ AdjACW = (9-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
}
- if(rpm > 849 and rpm < 975)
+ if(rpm > 239 and rpm <295)
+ {
+ AdjCW = (61+n);
+ AdjACW = (8-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
+ }
+ if(rpm > 294 and rpm <325)
{
- AdjCW = 3;
- AdjACW = 2;
+ AdjCW = (62+n);
+ AdjACW = (7-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
}
- if(rpm > 974 and rpm < 1150)
+ if(rpm > 324 and rpm <355)
+ {
+ AdjCW = (63+n);
+ AdjACW = (6-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
+ }
+ if(rpm > 354 and rpm <415)
{
- AdjCW = 4;
- AdjACW = 1;
- }
- if(rpm > 1149 and rpm < 1375)
+ AdjCW = (0+n);
+ AdjACW = (5-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
+ }
+ if(rpm > 414 and rpm <595)
{
- AdjCW = 5;
- AdjACW = 0;
+ AdjCW = (1+n);
+ AdjACW = (4-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
}
- if(rpm > 1374 and rpm < 1725)
+ if(rpm > 594 and rpm <655)
{
- AdjCW = 6;
- AdjACW = 63;
+ AdjCW = (2+n);
+ AdjACW = (3-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
}
- if(rpm > 1724 and rpm < 2150)
+ if(rpm > 654 and rpm <765)
{
- AdjCW = 7;
- AdjACW = 62;
+ AdjCW = (3+n);
+ AdjACW = (2-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
}
- if(rpm > 2149 and rpm < 2525)
+ if(rpm > 764 and rpm <1010)
+ {
+ AdjCW = (4+n);
+ AdjACW = (1-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
+ }
+ if(rpm > 1009 and rpm <1150)
{
- AdjCW = 8;
- AdjACW = 61;
+ AdjCW = (5+n);
+ AdjACW = (0-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
}
- if(rpm > 2524 and rpm < 2600)
+ if(rpm > 1149 and rpm <1225)
+ {
+ AdjCW = (6+n);
+ AdjACW = (63-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
+ }
+ if(rpm > 1224 and rpm <1310)
{
- AdjCW = 9;
- AdjACW = 60;
+ AdjCW = (7+n);
+ AdjACW = (62-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
}
- if(rpm > 2599 and rpm < 2650)
+ if(rpm > 1309 and rpm <1670)
{
- AdjCW = 10;
- AdjACW = 59;
+ AdjCW = (8+n);
+ AdjACW = (61-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
+ }
+ if(rpm > 1669 and rpm <2035)
+ {
+ AdjCW = (9+n);
+ AdjACW = (60-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
}
- if(rpm > 2649 and rpm < 2700)
+ if(rpm > 2034 and rpm <2175)
{
- AdjCW = 11;
- AdjACW = 58;
+ AdjCW = (10+n);
+ AdjACW = (59-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
}
- if(rpm > 2699 and rpm < 2750)
+ if(rpm > 2174 and rpm <2275)
{
- AdjCW = 12;
- AdjACW = 57;
+ AdjCW = (11+n);
+ AdjACW = (58-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
}
- if(rpm > 2749 and rpm < 2825)
+ if(rpm > 2274 and rpm <2515)
+ {
+ AdjCW = (12+n);
+ AdjACW = (57-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
+ }
+ if(rpm > 2514 and rpm <2575)
{
- AdjCW = 13;
- AdjACW = 56;
+ AdjCW = (13+n);
+ AdjACW = (56-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
}
- if(rpm > 2824 and rpm < 2875)
+ if(rpm > 2574 and rpm <2755)
+ {
+ AdjCW = (14+n);
+ AdjACW = (55-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
+ }
+ if(rpm > 2754 and rpm <2835)
{
- AdjCW = 14;
- AdjACW = 55;
+ AdjCW = (15+n);
+ AdjACW = (54-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
}
- if(rpm > 2874)
+ if(rpm > 2834 and rpm <2860)
{
- AdjCW = 15;
- AdjACW = 54;
+ AdjCW = (16+n);
+ AdjACW = (53-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
}
+ if(rpm > 2861)
+ {
+ AdjCW = (17+n);
+ AdjACW = (52-n);
+ if (AdjCW >(s-1))
+ {
+ AdjCW = 0;
+ }
+ if (AdjCW <0)
+ {
+ AdjCW = (s-1);
+ }
+ if (AdjACW <0)
+ {
+ AdjACW = (s-1);
+ }
+ if (AdjACW >(s-1))
+ {
+ AdjACW = 0;
+ }
+ }*/
//pc.printf("RPS = %i \r", RPS);
//pc.printf("StateA= %i, StateB= %i, StateC= %i, Pulses = %i\n\r", StateA, StateB, StateC, wheel.getPulses());
}
@@ -974,49 +1549,30 @@
{
diff = SetPoint - rpm; //difference between setpoint and the RPM measurement
duty = duty + (diff*AdjDiff); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. duty is adjusted to speed up or slow down until difference = 0
- /*if(duty > 0.8)
- {
- duty = duty + (diff*0.000001); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. duty is adjusted to speed up or slow down until difference = 0
- }
- if(0.5 < duty < 0.8)
- {
- duty = duty + (diff*0.005); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. duty is adjusted to speed up or slow down until difference = 0
- }
- if(duty < 0.5)
- {
- duty = duty + (diff*0.001); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. duty is adjusted to speed up or slow down until difference = 0
- }
- /*if(0 < rpm < 500)
- {
- duty = duty + (diff*0.0001); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. duty is adjusted to speed up or slow down until difference = 0
- }
- if(500 < rpm < 1000)
+ /*if (duty < 0.8)
{
- duty = duty + (diff*0.00008); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. duty is adjusted to speed up or slow down until difference = 0
+ j = 0;
+ k = 0;
+ l = 0;
+ m = 0;
+ n = 0;
}
- if(1000 < rpm < 1500)
- {
- duty = duty + (diff*0.00006); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. duty is adjusted to speed up or slow down until difference = 0
- }
- if(1500 < rpm < 2000)
+ if (duty > 0.79)
{
- duty = duty + (diff*0.00004); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. duty is adjusted to speed up or slow down until difference = 0
- }
- if(2000 < rpm < 2500)
- {
- duty = duty + (diff*0.00002); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. duty is adjusted to speed up or slow down until difference = 0
- }
- if(2500 < rpm < 3000)
- {
- duty = duty + (diff*0.00001); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. duty is adjusted to speed up or slow down until difference = 0
+ i = n-5;
+ j = n-4;
+ k = n-3;
+ l = n-2;
+ m = n-1;
+ n = 6;
}*/
if (duty > 1) //limits for duty. Motor will not operate below 0.96. 1 = max
{
duty = 1;
}
- if (duty <0.1) //3A min duty 0.96, 6.5A min duty 0.4
+ if (duty <0.08) //3A min duty 0.96, 6.5A min duty 0.4
{
- duty = 0.1;
+ duty = 0.08;
}
//pc.printf("%i, %.5f, %i, %i, %i \r", SetPoint, duty, AdjCW, AdjACW, rpm); //SetPoint = %i, rpm = %i\n\r", duty, SetPoint, rpm);
}
@@ -1028,11 +1584,11 @@
cs1 = 0;
SerialClock = 0; //set clock to 0
- wait_ms(1);
+ wait_ms(0.01);
SerialClock = 1; //clock once to set to the 13 bit temp data
- wait_ms(0.1);
+ wait_ms(0.01);
SerialClock = 0;
- wait_ms(0.1);
+ wait_ms(0.01);
for(i = 13; i > -1; i = i-1) // now data is temp data where MSB is first and each count is worth 0.25 degrees
{
@@ -1046,12 +1602,13 @@
}
SerialClock = 1; //clock to the next bit
- wait_ms(0.1);
+ wait_ms(0.01);
SerialClock = 0;
- wait_ms(0.1);
+ wait_ms(0.01);
}
temp = Readout * 0.125; //get the real temp value which is a float
- //pc.printf("%f\n\r",temp);
+ //pc.printf("%.1f\r",temp);
+ //pc.printf("%i, %.4f\n\r", rpm, duty);
Readout = 0;
cs1 = 1;
}
\ No newline at end of file