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.
main.cpp
- Committer:
- simontruelove
- Date:
- 2019-01-25
- Revision:
- 15:12a4bbfa6de4
- Parent:
- 14:1eb49362a607
- Child:
- 16:e6c8df9960c6
File content as of revision 15:12a4bbfa6de4:
#include "mbed.h"
#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 Ph0(void);
void Ph1(void);
void Ph12 (void);
void Ph2(void);
void Ph23 (void);
void Ph3(void);
void Ph34 (void);
void Ph4(void);
void Ph41 (void);
void GetChar (void);
void RPM (void);
void VelocityLoop (void);
void ReadKType(void);
Serial pc(USBTX, USBRX); // tx, rx - set up the Terraterm input from mbed
QEI wheel(p5, p6, p8, 800, QEI::X4_ENCODING); //code for quadrature encoder see QEI.h
Timer t; //timer used in RPM
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);
//AnalogOut Aout(p18); //Used with multimeter to give a speed indicator 1mV = 1RPM
//DigitalIn Button1 (p11); //not used
//DigitalIn Button2 (p12); //not used
DigitalOut led1(LED1); //LEDs used to as very basic memmory for controlling the state machines
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
DigitalOut SerialClock (p12); //ReadKType
DigitalIn DOut (p13); //ReadKType
DigitalOut cs1 (p14); //ReadKType
int StateA = 0; //State for first 2 revolutions (calibration of the index)
int StateB = 0; //All state machines after calibration use this state
//int StateC = 0;
int AdjCW = 57; //2 CW offset to adjust phase firing to give the fastest speed = used to calculate stateB
int AdjACW = 12; //5 ACW offset to adjust phase firing to give the fastest speed = used to calculate stateB
int CW = 57;
int ACW = 12;
int TimePerClick = 0; //for calc of RPM
int RPS = 0; //for calc of RPM
int rpm = 0; //for calc of RPM
int SetPoint = 1500; //for adjusting the speed
int enc = 3200;
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
int slowloop = 0;
char c; //keyboard cotrol GetChar
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 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
float temp = 0; //ReadKType
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);
wait(0.1);
t.start();
SerialClock = 0;
StepCW();
Initialisation();
/*while(1)
{
}
*/
while(wheel.getRevolutions()<2) //Index Calibration
{
StateA = (wheel.getPulses()+25)%s;
switch(StateA)
{
case 0:Ph1();break;//;pc.printf("1 Pulses= %i\n\r", wheel.getPulses());break;
case 1:Ph1();break;
case 2:Ph1();break;
case 3:Ph1();break;
case 4:Ph1();break;
case 5:Ph1();break;
case 6:Ph1();break;
case 7:Ph1();break;
case 8:Ph1();break;
case 9:Ph1();break;
case 10:Ph1();break;
case 11:Ph1();break;
case 12:Ph1();break;
case 13:Ph1();break;
case 14:Ph1();break;
case 15:Ph1();break;
case 16:Ph2();break;//;pc.printf("2 Pulses= %i\n\r", wheel.getPulses());break;
case 17:Ph2();break;
case 18:Ph2();break;
case 19:Ph2();break;
case 20:Ph2();break;
case 21:Ph2();break;
case 22:Ph2();break;
case 23:Ph2();break;
case 24:Ph2();break;
case 25:Ph2();break;
case 26:Ph2();break;
case 27:Ph2();break;
case 28:Ph2();break;
case 29:Ph2();break;
case 30:Ph2();break;
case 31:Ph2();break;
case 32:Ph3();break;//;pc.printf("3 Pulses= %i\n\r", wheel.getPulses());break;
case 33:Ph3();break;
case 34:Ph3();break;
case 35:Ph3();break;
case 36:Ph3();break;
case 37:Ph3();break;
case 38:Ph3();break;
case 39:Ph3();break;
case 40:Ph3();break;
case 41:Ph3();break;
case 42:Ph3();break;
case 43:Ph3();break;
case 44:Ph3();break;
case 45:Ph3();break;
case 46:Ph3();break;
case 47:Ph3();break;
case 48:Ph4();break;//;pc.printf("4 Pulses= %i\n\r", wheel.getPulses());break;
case 49:Ph4();break;
case 50:Ph4();break;
case 51:Ph4();break;
case 52:Ph4();break;
case 53:Ph4();break;
case 54:Ph4();break;
case 55:Ph4();break;
case 56:Ph4();break;
case 57:Ph4();break;
case 58:Ph4();break;
case 59:Ph4();break;
case 60:Ph4();break;
case 61:Ph4();break;
case 62:Ph4();break;
case 63:Ph4();break;
default:break;
}
if(wheel.getYay()==1) //PulseCount_==1, yay_=1;
{
StateA++;
wheel.ResetYay();
if (StateA>(s-1))
{
StateA=0;
}
}
}
while(1)
{
while((led1 == 0) && (led2 == 0)) //If no command to operate
{
//Aout = 0;
//duty = 0.5;
rpm = 0;
AdjCW = CW;
AdjACW = ACW;
Ph0();
GetChar();
ReadKType();
StateB = (wheel.getPulses()+StateA)%s; //wheel.getPulses()%1(s-1);
//StateC = (800+wheel.getPulses()+StateA+AdjCW)%s;
//if(wheel.getPulses()==wheel.getPulses()+1);
//{
//pc.printf("B StateA= %i, Pulses= %i, Revs= %i\r", StateA,wheel.getPulses(),wheel.getRevolutions());
//pc.printf("StateA= %i, StateB= %i, Pulses = %i \r", StateA, StateB, wheel.getPulses());
//}
//pc.printf("0 StateB= %i, Pulses= %i, Revs= %i\r", StateB,wheel.getPulses(),wheel.getRevolutions());
}
while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led1==1) && (temp<T)) //After Calibration, Prev CW movement, CW command
{
GetChar();
StateB = (wheel.getPulses()+StateA+AdjCW)%s;
//pc.printf("rpm = %i, whoop = %i\n\r", rpm, wheel.getWhoop());
//pc.printf("StateB= %i\n\r", StateB);
//pc.printf("1 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
switch(StateB)
{
case 0:Ph1();break;
case 1:Ph1();break;
case 2:Ph1();break;
case 3:Ph1();break;
case 4:Ph1();break;
case 5:Ph1();break;
case 6:Ph1();break;
case 7:Ph1();break;
case 8:Ph1();break;
case 9:Ph1();break;
case 10:Ph1();break;
case 11:Ph1();break;
case 12:Ph1();break;
case 13:Ph1();break;
case 14:Ph1();break;
case 15:Ph1();break;
case 16:Ph2();break;
case 17:Ph2();break;
case 18:Ph2();break;
case 19:Ph2();break;
case 20:Ph2();break;
case 21:Ph2();break;
case 22:Ph2();break;
case 23:Ph2();break;
case 24:Ph2();break;
case 25:Ph2();break;
case 26:Ph2();break;
case 27:Ph2();break;
case 28:Ph2();break;
case 29:Ph2();break;
case 30:Ph2();break;
case 31:Ph2();break;
case 32:Ph3();break;
case 33:Ph3();break;
case 34:Ph3();break;
case 35:Ph3();break;
case 36:Ph3();break;
case 37:Ph3();break;
case 38:Ph3();break;
case 39:Ph3();break;
case 40:Ph3();break;
case 41:Ph3();break;
case 42:Ph3();break;
case 43:Ph3();break;
case 44:Ph3();break;
case 45:Ph3();break;
case 46:Ph3();break;
case 47:Ph3();break;
case 48:Ph4();break;
case 49:Ph4();break;
case 50:Ph4();break;
case 51:Ph4();break;
case 52:Ph4();break;
case 53:Ph4();break;
case 54:Ph4();break;
case 55:Ph4();break;
case 56:Ph4();break;
case 57:Ph4();break;
case 58:Ph4();break;
case 59:Ph4();break;
case 60:Ph4();break;
case 61:Ph4();break;
case 62:Ph4();break;
case 63:Ph4();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;
{
RPM();
VelocityLoop();
slowloop++;
if(slowloop>rpm)
{
ReadKType();
slowloop=0;
}
}
}
while(wheel.getRevolutions()>1 && wheel.getPulses()<1 && (led1==1) && (temp<T)) //After Calibration, Prev ACW movement, CW command
{
GetChar();
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());
switch(StateB)
{
case 0:Ph1();break;
case 1:Ph1();break;
case 2:Ph1();break;
case 3:Ph1();break;
case 4:Ph1();break;
case 5:Ph1();break;
case 6:Ph1();break;
case 7:Ph1();break;
case 8:Ph1();break;
case 9:Ph1();break;
case 10:Ph1();break;
case 11:Ph1();break;
case 12:Ph1();break;
case 13:Ph1();break;
case 14:Ph1();break;
case 15:Ph1();break;
case 16:Ph2();break;
case 17:Ph2();break;
case 18:Ph2();break;
case 19:Ph2();break;
case 20:Ph2();break;
case 21:Ph2();break;
case 22:Ph2();break;
case 23:Ph2();break;
case 24:Ph2();break;
case 25:Ph2();break;
case 26:Ph2();break;
case 27:Ph2();break;
case 28:Ph2();break;
case 29:Ph2();break;
case 30:Ph2();break;
case 31:Ph2();break;
case 32:Ph3();break;
case 33:Ph3();break;
case 34:Ph3();break;
case 35:Ph3();break;
case 36:Ph3();break;
case 37:Ph3();break;
case 38:Ph3();break;
case 39:Ph3();break;
case 40:Ph3();break;
case 41:Ph3();break;
case 42:Ph3();break;
case 43:Ph3();break;
case 44:Ph3();break;
case 45:Ph3();break;
case 46:Ph3();break;
case 47:Ph3();break;
case 48:Ph4();break;
case 49:Ph4();break;
case 50:Ph4();break;
case 51:Ph4();break;
case 52:Ph4();break;
case 53:Ph4();break;
case 54:Ph4();break;
case 55:Ph4();break;
case 56:Ph4();break;
case 57:Ph4();break;
case 58:Ph4();break;
case 59:Ph4();break;
case 60:Ph4();break;
case 61:Ph4();break;
case 62:Ph4();break;
case 63:Ph4();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;
{
RPM();
VelocityLoop();
slowloop++;
if(slowloop>rpm)
{
ReadKType();
slowloop=0;
}
}
}
while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led2==1) && (temp<T)) //After Calibration, Prev CW movement, ACW command
{
GetChar();
//StateB = (800+wheel.getPulses())%16;
StateB = (wheel.getPulses()+StateA+AdjACW)%s;
//pc.printf("StateA= %i\r", StateA);
//pc.printf("3 StateB= %i, Pulses= %i, Revs= %i \n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
switch(StateB)
{
case 63:Ph4();break;
case 62:Ph4();break;
case 61:Ph4();break;
case 60:Ph4();break;
case 59:Ph4();break;
case 58:Ph4();break;
case 57:Ph4();break;
case 56:Ph4();break;
case 55:Ph4();break;
case 54:Ph4();break;
case 53:Ph4();break;
case 52:Ph4();break;
case 51:Ph4();break;
case 50:Ph4();break;
case 49:Ph4();break;
case 48:Ph4();break;
case 47:Ph3();break;
case 46:Ph3();break;
case 45:Ph3();break;
case 44:Ph3();break;
case 43:Ph3();break;
case 42:Ph3();break;
case 41:Ph3();break;
case 40:Ph3();break;
case 39:Ph3();break;
case 38:Ph3();break;
case 37:Ph3();break;
case 36:Ph3();break;
case 35:Ph3();break;
case 34:Ph3();break;
case 33:Ph3();break;
case 32:Ph3();break;
case 31:Ph2();break;
case 30:Ph2();break;
case 29:Ph2();break;
case 28:Ph2();break;
case 27:Ph2();break;
case 26:Ph2();break;
case 25:Ph2();break;
case 24:Ph2();break;
case 23:Ph2();break;
case 22:Ph2();break;
case 21:Ph2();break;
case 20:Ph2();break;
case 19:Ph2();break;
case 18:Ph2();break;
case 17:Ph2();break;
case 16:Ph2();break;
case 15:Ph1();break;
case 14:Ph1();break;
case 13:Ph1();break;
case 12:Ph1();break;
case 11:Ph1();break;
case 10:Ph1();break;
case 9:Ph1();break;
case 8:Ph1();break;
case 7:Ph1();break;
case 6:Ph1();break;
case 5:Ph1();break;
case 4:Ph1();break;
case 3:Ph1();break;
case 2:Ph1();break;
case 1:Ph1();break;
case 0:Ph1();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;
{
RPM();
VelocityLoop();
slowloop++;
if(slowloop>rpm)
{
ReadKType();
slowloop=0;
}
}
}
while((wheel.getRevolutions()>1) && (wheel.getPulses()<1) && (led2==1) &&(temp<T)) //After Calibration, Prev ACW movement, ACW command
{
GetChar();
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());
switch(StateB)
{
case 63:Ph4();break;
case 62:Ph4();break;
case 61:Ph4();break;
case 60:Ph4();break;
case 59:Ph4();break;
case 58:Ph4();break;
case 57:Ph4();break;
case 56:Ph4();break;
case 55:Ph4();break;
case 54:Ph4();break;
case 53:Ph4();break;
case 52:Ph4();break;
case 51:Ph4();break;
case 50:Ph4();break;
case 49:Ph4();break;
case 48:Ph4();break;
case 47:Ph3();break;
case 46:Ph3();break;
case 45:Ph3();break;
case 44:Ph3();break;
case 43:Ph3();break;
case 42:Ph3();break;
case 41:Ph3();break;
case 40:Ph3();break;
case 39:Ph3();break;
case 38:Ph3();break;
case 37:Ph3();break;
case 36:Ph3();break;
case 35:Ph3();break;
case 34:Ph3();break;
case 33:Ph3();break;
case 32:Ph3();break;
case 31:Ph2();break;
case 30:Ph2();break;
case 29:Ph2();break;
case 28:Ph2();break;
case 27:Ph2();break;
case 26:Ph2();break;
case 25:Ph2();break;
case 24:Ph2();break;
case 23:Ph2();break;
case 22:Ph2();break;
case 21:Ph2();break;
case 20:Ph2();break;
case 19:Ph2();break;
case 18:Ph2();break;
case 17:Ph2();break;
case 16:Ph2();break;
case 15:Ph1();break;
case 14:Ph1();break;
case 13:Ph1();break;
case 12:Ph1();break;
case 11:Ph1();break;
case 10:Ph1();break;
case 9:Ph1();break;
case 8:Ph1();break;
case 7:Ph1();break;
case 6:Ph1();break;
case 5:Ph1();break;
case 4:Ph1();break;
case 3:Ph1();break;
case 2:Ph1();break;
case 1:Ph1();break;
case 0:Ph1();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;
{
RPM();
VelocityLoop();
slowloop++;
if(slowloop>rpm)
{
ReadKType();
slowloop=0;
}
}
}
while(temp>(T-1))
{
Initialisation();
pc.printf("Motor Over Temp\n\r");
while(1)
{
ReadKType();
pc.printf("%f\r",temp);
wait(1);
if(temp<T-20)
{
pc.printf("Motor Back Online\n\r");
break;
}
}
}
}
}
void StepCW(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());
Ph34();
wait(y);
//pc.printf("34 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());
Ph23();
wait(y);
//pc.printf("23 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());
Ph12();
wait(y);
//pc.printf("12 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
Ph1();
wait(x);
//pc.printf("1 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());
}
void Ph0(void)
{
Phase1.write(0);
Phase2.write(0);
Phase3.write(0);
Phase4.write(0);
//wait(x);
//pc.printf("Phase 1 = %i\n\r", wheel.getPulses());
}
void Ph1(void)
{
Phase1.write(duty);
Phase2.write(0);
Phase3.write(0);
Phase4.write(0);
//wait(x);
//pc.printf("Phase 1 = %i\n\r", wheel.getPulses());
}
void Ph12 (void)
{
Phase1.write(duty);
Phase2.write(duty);
Phase3.write(0);
Phase4.write(0);
//wait(y);
}
void Ph2(void)
{
Phase1.write(0);
Phase2.write(duty);
Phase3.write(0);
Phase4.write(0);
//wait(x);
//pc.printf("Phase 2 = %i\n\r", wheel.getPulses());
}
void Ph23 (void)
{
Phase1.write(0);
Phase2.write(duty);
Phase3.write(duty);
Phase4.write(0);
//wait(y);
}
void Ph3(void)
{
Phase1.write(0);
Phase2.write(0);
Phase3.write(duty);
Phase4.write(0);
//wait(x);
//pc.printf("Phase 3 = %i\n\r", wheel.getPulses());
}
void Ph34 (void)
{
Phase1.write(0);
Phase2.write(0);
Phase3.write(duty);
Phase4.write(duty);
//wait(y);
}
void Ph4(void)
{
Phase1.write(0);
Phase2.write(0);
Phase3.write(0);
Phase4.write(duty);
//wait(x);
//pc.printf("Phase 4 = %i\n\r", wheel.getPulses());
}
void Ph41 (void)
{
Phase1.write(duty);
Phase2.write(0);
Phase3.write(0);
Phase4.write(duty);
//wait(y);
}
void Initialisation (void) //Turn everything off
{
Phase1.write(0);
Phase2.write(0);
Phase3.write(0);
Phase4.write(0);
led1 = 0;
led2 = 0;
led3 = 0;
led4 = 0;
UnUsedPhase1=0;
UnUsedPhase2=0;
wheel.ResetYay();
wheel.QEI::reset();
}
void GetChar (void) //read keyboard strikes with terraterm
{ if (pc.readable())
{
c = pc.getc();
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)
{
SetPoint = 3000;
}
pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
}
if(c == 'a') //Decreases setpoint used in Velocity loop
{
//duty = duty - 0.0001;
SetPoint=SetPoint-5;
if (SetPoint <50)
{
SetPoint = 50;
}
pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
}
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);
}
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);
}
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);
}
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);
}
if(c== 'o')
{
AdjCW = AdjCW+1;
if (AdjCW >(s-1))
{
AdjCW = 0;
}
pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
}
if(c== 'k')
{
AdjCW = AdjCW-1;
if (AdjCW <0)
{
AdjCW = (s-1);
}
pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
}
if(c== 'p')
{
AdjACW = AdjACW+1;
if (AdjACW >(s-1))
{
AdjACW = 0;
}
pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
}
if(c== 'l')
{
AdjACW = AdjACW-1;
if (AdjACW <0)
{
AdjACW = (s-1);
}
pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
}
if(c=='0')
{
pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
}
if(c=='t')
{
pc.printf("%.0f C\n\r",temp);
}
if(c=='y')
{
pc.printf("3 StateB= %i, Pulses= %i, Revs= %i \n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
}
}
}
void RPM (void)
{
wheel.ResetWhoop(); //PulseCount2_==400 x 4, x4 encoding, whoop_=1;
TimePerClick = (t.read_us()); //read timer in microseconds
t.reset(); //reset timer
TimePerRev = enc * TimePerClick / z; //z = 400 (PulseCount2_==1600)
TimePerRev = TimePerRev / 1000000; // 1microsecond = 0.000001s
RPS = 1 / TimePerRev; //inverse to convert SPR to RPS
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)
{
AdjCW = 57;
AdjACW = 12;
}
if(rpm > 299 && 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;
}
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;
}
//pc.printf("RPS = %i \r", RPS);
//pc.printf("StateA= %i, StateB= %i, StateC= %i, Pulses = %i\n\r", StateA, StateB, StateC, wheel.getPulses());
}
void VelocityLoop (void)
{
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)
{
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
}
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)
{
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
}*/
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
{
duty = 0.1;
}
//pc.printf("%i, %.5f, %i, %i, %i \r", SetPoint, duty, AdjCW, AdjACW, rpm); //SetPoint = %i, rpm = %i\n\r", duty, SetPoint, rpm);
}
void ReadKType(void)
{
int i = 0;
int Readout = 0;
cs1 = 0;
SerialClock = 0; //set clock to 0
wait_ms(1);
SerialClock = 1; //clock once to set to the 13 bit temp data
wait_ms(0.1);
SerialClock = 0;
wait_ms(0.1);
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
{
if(DOut == 1) //check data, store results in readout
{
Readout |= (1<<i);
}
else
{
Readout |= (0<<i);
}
SerialClock = 1; //clock to the next bit
wait_ms(0.1);
SerialClock = 0;
wait_ms(0.1);
}
temp = Readout * 0.125; //get the real temp value which is a float
//pc.printf("%f\n\r",temp);
Readout = 0;
cs1 = 1;
}