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.
Revision 24:ebaced951bbd, committed 2021-05-19
- Comitter:
- simontruelove
- Date:
- Wed May 19 10:43:30 2021 +0000
- Parent:
- 23:9d85fc0217c5
- Commit message:
- 1st Stab
Changed in this revision
--- a/QEI.lib Tue May 21 14:38:56 2019 +0000 +++ b/QEI.lib Wed May 19 10:43:30 2021 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/simontruelove/code/QEI/#a6bd9410dcbc +https://os.mbed.com/users/simontruelove/code/QEI/#ee2907e1266e
--- a/TextLCD.lib Tue May 21 14:38:56 2019 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://os.mbed.com/users/simon/code/TextLCD/#308d188a2d3a
--- a/main.cpp Tue May 21 14:38:56 2019 +0000
+++ b/main.cpp Wed May 19 10:43:30 2021 +0000
@@ -1,41 +1,15 @@
#include "mbed.h"
#include "QEI.h"
-#include "TextLCD.h"
-/* Code written by Simon Truelove (completed 5/4/2019) to control Switched Reluctance Motor (SRM) modified from a
-QSH6018-65-28-210 stepper motor. The position of the motor is monitored using a rotary encoder 102-1307-ND using QEI.h.
-The code uses the pulses from the encoder to increment the values StateA and StateB to control switching of the phases needed
-to generate rotation via a 64 case state machine. Upon start up the motor will step anti clockwise by switching each phase
-on in sequence then it will complete 2 revolutions to find the encoder index and set StateA which is used for calculating
-StateB. StateB controls 4 identical state machines, however state B is calculated slightly differently within each one
-depending on the direction of rotation required and the direction the motor had turned previously. LED's are switched on and
-off and these LED's control which of the 4 state machines is used. When the motor is running the rpm is calculated every rotation
-and then the PWM duty is adjusted until the desired rpm is achieved. The rpm is also used to change the phase advance and the PWM gain
-so that it runs with the optimum setting at any speed and the changes in PWM duty give a smooth change in speed. The temperature
-of the thrmocouple is measured every 0.6 seconds and will stop the motor if the temperature exceeds 80C.
+/* Code written by Simon Truelove (18/05/2021) Stripped down version of SWM Demonstrator to check operation of TFM encoder RLS LM13ICPRGA00D10A and MR100F085A160M00
+using QEI.h. The code uses the pulses from the encoder to calculate RPM
*/
-void Initialisation (void); //These function prototypes are written after the main. They must be listed here too
-void StepACW(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);
-void Switch (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
-TextLCD lcd(p9, p10, p16, p17, p18, p19, TextLCD::LCD16x2);
+QEI wheel(p5, p6, p8, 20000, QEI::X4_ENCODING); //code for quadrature encoder see QEI.h
Timer t; //timer used in RPM
@@ -44,724 +18,49 @@
DigitalOut led3 (LED3);
DigitalOut led4 (LED4);
DigitalIn anticlockwise (p11);
-DigitalOut SerialClock (p12); //ReadKType SPI
-DigitalIn DOut (p13); //ReadKType SPI
-DigitalOut cs1 (p14); //Chip Select SPI
DigitalIn clockwise (p15);
-AnalogIn pot (p20); //Potentiometer for adjusting speed SetPoint
DigitalOut UnUsedPhase1 (p21);
DigitalOut UnUsedPhase2 (p22);
-PwmOut Phase1 (p23);
-PwmOut Phase2 (p24); //Pin set up - PWM to enable speed control
-PwmOut Phase3 (p25);
-PwmOut Phase4 (p26);
-int SetPoint = 1500; //for adjusting the speed
-int StateA = 0; //State for first 2 revolutions (calibration of the index)
-int StateB = 0; //All state machines after calibration use this state
-int AdjCW = 57; //CW offset to adjust phase firing to give the fastest speed = used to calculate stateB
-int AdjACW = 12; //ACW offset to adjust phase firing to give the fastest speed = used to calculate stateB
-int CW = 57; //Value used to start motor from stationary
-int ACW = 12; //Value used to start motor from stationary
int TimePerClick = 0; //for calc of RPM
-int enc = 3200; //800 x4 enc = 3200 Pulses Per Rev
+int enc = 80000; //20 000 x4 enc = 80 000 Pulses Per Rev
int s = enc/50; //s=number of cases in state machine (64)
-int T = 80; //Thermostat protection limit degrees centigrade
-int z = 3200; //TimePerRev = TimePerClick * (3200/z); 3200 pulses per rev, PulseCount2_==3200 for wheel.getwhoop_ flag. i.e. 1 points per reoluition for RPM calc. //Motor temp limit
-int slowloop = 0; //Counter used to read temp at a slower rate
int start = 28;
int adj = 25-start;
char c; //keyboard cotrol GetChar
-float SetPointBuffer = 0; //Used in calculation to convert potentiometer value to SetPoint value. Used due to changing between int and float.
float rps = 0; //for calc of RPM
float rpm = 0; //for calc of RPM
-float duty = 0.5; //PWM duty
-float diff = 0.0; //Velocity loop: diff = SetPoint - rpm;
-float gain = 0.0001; //Velocity loop
-float p = 0.000014; //PWM period
-float x=0.3; //Used in StepACW x=time of square wave when 1 phase energised,
float TimePerRev = 0; //for calc of RPM
-float y=0.04; //Used in Step ACWy=time of square wave when 2 phases energised
-float temp = 0; //ReadKType
int main(void)
{
- pc.baud(921600); //Set fastest baud rate
- Phase1.period(p); //period of 0.000014 = 14 microseconds (500kHz). Good balance of low and high speed performance.
- Phase2.period(p);
- Phase3.period(p);
- Phase4.period(p);
- lcd.cls();
+ pc.baud(460800); //Set baud rate
wait(2);
- lcd.printf(" Switched\n");
- lcd.locate(0,1);
- lcd.printf("Reluctance Motor\n");
t.start(); //start timer for rpm calculation
- SerialClock = 0; //reset serial clock for Themocouple
- StepACW();
wait(0.1);
- //duty=1; //Makes sure the motor starts from the same position each time
- Initialisation(); //reset everything
-
- while(wheel.getRevolutions()<4) //Index Calibration
- {
- StateA = (wheel.getPulses()+start)%s;
- switch(StateA)
- {
- case 0:Ph1();pc.printf("1 %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();pc.printf("2 %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;
- case 33:Ph3();pc.printf("3 %i\n\r", wheel.getPulses());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();pc.printf("4 %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 used to increment the StateA count
- {
- StateA++; //increment StateA
- wheel.ResetYay(); //reset Yay
- if (StateA>(s-1)) //State A is only valid between 0 and 63 (64 states)
- {
- StateA=0; //continuous loop
- }
- }
- }
while(1)
{
- while((clockwise == 0) && (anticlockwise == 0)) //If no command to operate
- {
- duty = 0.11; //Duty reduced to low value to ensure ramp up of speed
- rpm = 0; //RPM function not being triggered due to no rotation. RPM set to 0
- SetPointBuffer = (float) 3000 *pot; //cast pot value as a float
- SetPoint = (int) SetPointBuffer; //Convert SetPointBuffer to an int and use as value for SetPoint
- AdjCW = CW; //Reset to correct value for start up
- AdjACW = ACW; //Reset to correct value for start up
- Ph0(); //turn off all phases
- GetChar(); //read keyboard strikes
- ReadKType(); //Temperature measurement
- StateB = (wheel.getPulses()+StateA)%s; //calculation for stateB
- Switch();
- lcd.cls();
- wait(0.001);
- lcd.printf("SRM STOP");
- lcd.locate(0,1);
- lcd.printf("%.0fRPM, %.0fdegC\n",rpm, temp);
- wait(0.1);
- }
- while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (clockwise==1) && (temp<T)) //After Calibration, Prev CW movement, CW command
- {
- GetChar();
- Switch(); //read keyboard strikes
- StateB = (wheel.getPulses()+StateA+AdjCW-adj)%s; //calculation for stateB
- SetPointBuffer = (float) 3000 *pot; //cast pot value as a float
- SetPoint = (int) SetPointBuffer; //Convert SetPointBuffer to an int and use as value for SetPoint
-
- switch(StateB)
- {
- case 0:Ph1();break;
- case 1:Ph1();break;
- case 2:Ph1();break;z
- 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:Ph0();break;
- case 11:Ph1();break;
- case 12:Ph1();break;
- case 13:Ph0();break;
- case 14:Ph1();break;
- case 15:Ph0();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:Ph0();break;
- case 27:Ph2();break;
- case 28:Ph2();break;
- case 29:Ph0();break;
- case 30:Ph2();break;
- case 31:Ph0();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:Ph0();break;
- case 43:Ph3();break;
- case 44:Ph3();break;
- case 45:Ph0();break;
- case 46:Ph3();break;
- case 47:Ph0();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:Ph0();break;
- case 59:Ph4();break;
- case 60:Ph4();break;
- case 61:Ph0();break;
- case 62:Ph4();break;
- case 63:Ph0();break;
- default:break;
- }
-
- if(wheel.getWhoop()==1) //PulseCount2_==3200 (this is 1 rotation), whoop_=1;
+ if(wheel.getWhoop()==1) //PulseCount2_==3200 (this is 1 rotation), whoop_=1;
{
RPM(); //calculate RPM
- VelocityLoop(); //Adjust velocity
- slowloop++; //increment slowloop
- if(slowloop>(0.02*rpm)) //Reads temperature at a constant time interval
- {
- ReadKType(); //Read Temp
- lcd.cls();
- wait(0.0001);
- lcd.printf("CW %.0f\n",rpm);
- slowloop=0; //Reset slowloop
- }
+ led1==1;
+ pc.printf("yay= %i, whoop= %i, Pulses= %i, Revs= %i, RPM= %f\n\r", wheel.getYay(),wheel.getWhoop(),wheel.getPulses(),wheel.getRevolutions(), rpm);
+ }
+ if (pc.readable())
+ {
+ c = pc.getc();
+ if(c == 'z') //turn on led2 print some stuff
+ {
+ led2 = !led2;
+ pc.printf("yay= %i, whoop= %i, Pulses= %i, Revs= %i, RPM= %f\n\r", wheel.getYay(),wheel.getWhoop(),wheel.getPulses(),wheel.getRevolutions(), rpm);
}
}
-
- while(wheel.getRevolutions()>1 && wheel.getPulses()<1 && (clockwise==1) && (temp<T)) //After Calibration, Prev ACW movement, CW command
- {
- GetChar();
- Switch();
- StateB = (enc+wheel.getPulses()+StateA+AdjCW-adj)%s;
- SetPointBuffer = (float) 3000 *pot;
- SetPoint = (int) SetPointBuffer;
-
- 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:Ph0();break;
- case 11:Ph1();break;
- case 12:Ph1();break;
- case 13:Ph0();break;
- case 14:Ph1();break;
- case 15:Ph0();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:Ph0();break;
- case 27:Ph2();break;
- case 28:Ph2();break;
- case 29:Ph0();break;
- case 30:Ph2();break;
- case 31:Ph0();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:Ph0();break;
- case 43:Ph3();break;
- case 44:Ph3();break;
- case 45:Ph0();break;
- case 46:Ph3();break;
- case 47:Ph0();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:Ph0();break;
- case 59:Ph4();break;
- case 60:Ph4();break;
- case 61:Ph0();break;
- case 62:Ph4();break;
- case 63:Ph0();break;
- default:break;
- }
-
- if(wheel.getWhoop()==1)
- {
- RPM();
- VelocityLoop();
- slowloop++;
- if(slowloop>(0.01*rpm))
- {
- ReadKType();
- lcd.cls();
- wait(0.0001);
- lcd.printf("CW %.0f\n",rpm);
- slowloop=0;
- }
- }
- }
- while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (anticlockwise==1) && (temp<T)) //After Calibration, Prev CW movement, ACW command
- {
- GetChar();
- Switch();
- StateB = (wheel.getPulses()+StateA+AdjACW-adj)%s;
- SetPointBuffer = (float) 3000 *pot;
- SetPoint = (int) SetPointBuffer;
-
- 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:Ph0();break;
- case 52:Ph4();break;
- case 51:Ph4();break;
- case 50:Ph0();break;
- case 49:Ph4();break;
- case 48:Ph0();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:Ph0();break;
- case 36:Ph3();break;
- case 35:Ph3();break;
- case 34:Ph0();break;
- case 33:Ph3();break;
- case 32:Ph0();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:Ph0();break;
- case 20:Ph2();break;
- case 19:Ph2();break;
- case 18:Ph0();break;
- case 17:Ph2();break;
- case 16:Ph0();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:Ph0();break;
- case 4:Ph1();break;
- case 3:Ph1();break;
- case 2:Ph0();break;
- case 1:Ph1();break;
- case 0:Ph0();break;
- default:break;
- }
- if(wheel.getWhoop()==1)
- {
- RPM();
- VelocityLoop();
- slowloop++;
- if(slowloop>(0.01*rpm))
- {
- ReadKType();
- lcd.cls();
- wait(0.0001);
- lcd.printf("ACW %.0f\n",rpm);
- slowloop=0;
- }
- }
- }
- while((wheel.getRevolutions()>1) && (wheel.getPulses()<1) && (anticlockwise==1) &&(temp<T)) //After Calibration, Prev ACW movement, ACW command
- {
- GetChar();
- Switch();
- StateB = (enc+wheel.getPulses()+StateA+AdjACW-adj)%s;
- SetPointBuffer = (float) 3000 *pot;
- SetPoint = (int) SetPointBuffer;
-
- 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:Ph0();break;
- case 52:Ph4();break;
- case 51:Ph4();break;
- case 50:Ph0();break;
- case 49:Ph4();break;
- case 48:Ph0();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:Ph0();break;
- case 36:Ph3();break;
- case 35:Ph3();break;
- case 34:Ph0();break;
- case 33:Ph3();break;
- case 32:Ph0();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:Ph0();break;
- case 20:Ph2();break;
- case 19:Ph2();break;
- case 18:Ph0();break;
- case 17:Ph2();break;
- case 16:Ph0();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:Ph0();break;
- case 4:Ph1();break;
- case 3:Ph1();break;
- case 2:Ph0();break;
- case 1:Ph1();break;
- case 0:Ph0();break;
- default:break;
- }
-
- if(wheel.getWhoop()==1)
- {
- RPM();
- VelocityLoop();
- slowloop++;
- if(slowloop>(0.01*rpm))
- {
- ReadKType();
- lcd.cls();
- wait(0.0001);
- lcd.printf("CW %.0f\n",rpm);
- slowloop=0;
- }
- }
- }
- while(temp>(T-1)) //If Temp exceeds T the motor will not operate
- {
- Initialisation();
- wait(0.001);
- lcd.printf("SRM Over Temp");
- lcd.locate(0,1);
- lcd.printf("RPM=%.0f, %.0f C\n",rpm, temp);
- wait(0.1);
- pc.printf("Motor Over Temp\n\r");
- while(1)
- {
- ReadKType(); //Read motor Temp
- lcd.printf("SRM Over Temp");
- lcd.locate(0,1);
- lcd.printf("RPM=%.0f, %.0f C\n",rpm, temp);
- wait(0.1);
- pc.printf("%f\r",temp); //Display current Temp
- wait(1);
- if(temp<T-20) //Reset when Temp 20 degrees C below thermostat trip value
- {
- wait(0.001);
- lcd.printf("SRM Back Online");
- lcd.locate(0,1);
- lcd.printf("RPM=%.0f, %.0f C\n",rpm, temp);
- wait(0.1);
- pc.printf("Motor Back Online\n\r");
- break;
- }
- }
- }
}
}
-void StepACW(void) //Square wave switching
-{
- 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) //Turn off all Phases
-{
- Phase1.write(0);
- Phase2.write(0);
- Phase3.write(0);
- Phase4.write(0);
-}
-
-void Ph1(void) //Turn on Phase 1
-{
- Phase1.write(duty);
- Phase2.write(0);
- Phase3.write(0);
- Phase4.write(0);
-}
-
-void Ph12 (void) //Turn on Phase 1 and 2
-{
- Phase1.write(duty);
- Phase2.write(duty);
- Phase3.write(0);
- Phase4.write(0);
-}
-
-void Ph2(void) //Turn on Phase 2
-{
- Phase1.write(0);
- Phase2.write(duty);
- Phase3.write(0);
- Phase4.write(0);
-}
-
-void Ph23 (void) //Turn on Phase 2 and 3
-{
- Phase1.write(0);
- Phase2.write(duty);
- Phase3.write(duty);
- Phase4.write(0);
-}
-
-void Ph3(void) //Turn on Phase 3
-{
- Phase1.write(0);
- Phase2.write(0);
- Phase3.write(duty);
- Phase4.write(0);
-}
-
-void Ph34 (void) //Turn on Phase 3 and 4
-{
- Phase1.write(0);
- Phase2.write(0);
- Phase3.write(duty);
- Phase4.write(duty);
-}
-
-void Ph4(void) //Turn on Phase 4
-{
- Phase1.write(0);
- Phase2.write(0);
- Phase3.write(0);
- Phase4.write(duty);
-}
-
-void Ph41 (void) //Turn on Phase 4 and 1
-{
- Phase1.write(duty);
- Phase2.write(0);
- Phase3.write(0);
- Phase4.write(duty);
-}
-
-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();
- switch(c)
- {
- case ' ': pc.printf(" 0 = Phase Mapping\n\rq = setpoint+, w = duty+, e = gain+, t = temp, y = states, o = AdjCW+, p = AdjACW+,\n\r a = setpoint-, s = duty-, d = gain-, k = AdjCW-, l = AdjACW-,\n\r z = CW, x = ACW\n\r");break;
- case '1': Ph1();break; //Check Phase 1
- case '2': Ph2();break; //Check Phase 2
- case '3': Ph3();break; //Check Phase 3
- case '4': Ph4();break; //Check Phase 4
- case '5': Ph0();break; //Turn off all Phases
- case '6': StepACW();break; //Step ACW
- case '7': pc.printf("TimePerClick = %i, TimePerRev = %.5f, rps = %.5f, rpm = %.1f, \n\r", TimePerClick, TimePerRev, rps, rpm);break; //Print rpm related data
- case '9': pc.printf("State A = %i, StateB = %i, Pulses= %i, Revs= %i \n\r", StateA, StateB, wheel.getPulses(), wheel.getRevolutions());break; //Print data relating to position
- case '0': pc.printf("%i, %.5f, %i, %i, %.1f, %f, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, gain, p);break; //Print general data
- case 'q': SetPoint=SetPoint+5;if (SetPoint >3000){SetPoint = 3000;}pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //Increases setpoint used in Velocity loop
- case 'a': SetPoint=SetPoint-5;if (SetPoint <0){SetPoint = 0;}pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //Decreases setpoint used in Velocity loop
- case 'w': duty = duty + 0.01;pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //Increases duty used in Velocity loop
- case 's': duty = duty - 0.01;pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //Decreases duty used in Velocity loop
- case 'e': gain = gain * 10;pc.printf("%i, %.5f, %i, %i, %.1f, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, gain);break; //Increases gain used in Velocity loop
- case 'd': gain = gain / 10;pc.printf("%i, %.5f, %i, %i, %.1f, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, gain);break; //Decreases gain used in Velocity loop
- case 'r': p = p+0.0000001;pc.printf("%i, %.5f, %i, %i, %i, %0.7f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, p);break; //Increasees PWM period
- case 'f': p = p-0.0000001;pc.printf("%i, %.5f, %i, %i, %i, %0.7f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, p);break; //Decreasees PWM period
- case 't': pc.printf("%.0f C\n\r",temp);break; //Print current Temp
- case 'o': AdjCW = AdjCW+1;if (AdjCW >(s-1)){AdjCW = 0;}pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //AdjCW+
- case 'k': AdjCW = AdjCW-1;if (AdjCW <0){AdjCW = (s-1);}pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //AdjCW-
- case 'p': AdjACW = AdjACW+1;if (AdjACW >(s-1)){AdjACW = 0;}pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //AdjACW+
- case 'l': AdjACW = AdjACW-1;if (AdjACW <0){AdjACW = (s-1);}pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //AdjACW-
- case 'z': led1 = !led1;led2 = 0;pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //Run motor CW
- case 'x': led1 = 0;led2 = !led2;pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //Run motor ACW
- case 'm': pc.printf("%f \n\r", pot.read()); //Potentiometer value
- }
- }
-}
-
void RPM (void)
{
wheel.ResetWhoop(); //Whoop = 1 x per rev
@@ -771,512 +70,4 @@
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
-
- //if(duty < 0.9) //Mapped Phase Advance with no load
- {
- if (rpm<299)
- {
- AdjCW = (57);
- AdjACW = (12);
- gain = (0.0001);
- }
- else if(rpm > 299 and rpm < 309)
- {
- AdjCW = (59);
- AdjACW = (12);
- gain = (0.0001);
- }
- else if(rpm > 309 and rpm < 319)
- {
- AdjCW = (60);
- AdjACW = (12);
- gain = (0.0001);
- }
- else if(rpm > 319 and rpm < 328)
- {
- AdjCW = (61);
- AdjACW = (12);
- gain = (0.0001);
- }
- else if(rpm > 328 and rpm < 337)
- {
- AdjCW = (62);
- AdjACW = (12);
- gain = (0.0001);
- }
- else if(rpm > 337 and rpm < 346)
- {
- AdjCW = (63);
- AdjACW = (12);
- gain = (0.0001);
- }
- else if(rpm > 346 and rpm < 351)
- {
- AdjCW = (0);
- AdjACW = (12);
- gain = (0.0001);
- }
- else if(rpm > 351 and rpm < 357)
- {
- AdjCW = (1);
- AdjACW = (11);
- gain = (0.0001);
- }
- else if(rpm > 357 and rpm < 362)
- {
- AdjCW = (3);
- AdjACW = (10);
- gain = (0.0001);
- }
- else if(rpm > 362 and rpm < 420)
- {
- AdjCW = (4);
- AdjACW = (6);
- gain = (0.0001);
- }
- else if(rpm > 420 and rpm < 548)
- {
- AdjCW = (5);
- AdjACW = (4);
- gain = (0.0001);
- }
- else if(rpm > 548 and rpm < 664)
- {
- AdjCW = (6);
- AdjACW = (3);
- gain = (0.0001);
- }
- else if(rpm > 664 and rpm < 710)
- {
- AdjCW = (7);
- AdjACW = (3);
- gain = (0.0001);
- }
- else if(rpm > 710 and rpm < 767)
- {
- AdjCW = (8);
- AdjACW = (2);
- gain = (0.0001);
- }
- else if(rpm > 767 and rpm < 960)
- {
- AdjCW = (9);
- AdjACW = (1);
- gain = (0.0001);
- }
- else if(rpm > 960 and rpm < 1051)
- {
- AdjCW = (10);
- AdjACW = (0);
- gain = (0.0001);
- }
- else if(rpm > 1051 and rpm < 1281)
- {
- AdjCW = (11);
- AdjACW = (63);
- gain = (0.00001);
- }
- else if(rpm > 1281 and rpm < 1427)
- {
- AdjCW = (12);
- AdjACW = (62);
- gain = (0.00001);
- }
- else if(rpm > 1427 and rpm < 1613)
- {
- AdjCW = (13);
- AdjACW = (61);
- gain = (0.00001);
- }
- else if(rpm > 1613 and rpm < 1661)
- {
- AdjCW = (14);
- AdjACW = (60);
- gain = (0.00001);
- }
- else if(rpm > 1661 and rpm < 1746)
- {
- AdjCW = (15);
- AdjACW = (59);
- gain = (0.00001);
- }
- else if(rpm > 1746 and rpm < 1999)
- {
- AdjCW = (16);
- AdjACW = (58);
- gain = (0.00001);
- }
- else if(rpm > 1999 and rpm < 2324)
- {
- AdjCW = (17);
- AdjACW = (57);
- gain = (0.00001);
- }
- else if(rpm > 2324 and rpm < 2621)
- {
- AdjCW = (18);
- AdjACW = (56);
- gain = (0.00001);
- }
- else if(rpm > 2621 and rpm < 2779)
- {
- AdjCW = (19);
- AdjACW = (55);
- gain = (0.00001);
- }
- else if(rpm > 2779 and rpm < 2882)
- {
- AdjCW = (20);
- AdjACW = (54);
- gain = (0.00001);
- }
- else if(rpm > 2882 and rpm < 2967)
- {
- AdjCW = (22);
- AdjACW = (53);
- gain = (0.000005);
- }
- else if(rpm > 2967)
- {
- AdjCW = (23);
- AdjACW = (52);
- gain = (0.000005);
- }
- }
- /*if(rpm < 135)
- {
- AdjCW = (0);
- AdjACW = (13);
- gain = (0.0001); //proportional gain
- }
- else if(rpm > 135.2 and rpm < 171)
- {
- AdjCW = (1);
- AdjACW = (13);
- gain = (0.0001);
- }
- else if(rpm > 171 and rpm < 188)
- {
- AdjCW = (1);
- AdjACW = (11);
- gain = (0.0001);
- }
- else if(rpm > 188 and rpm < 245)
- {
- AdjCW = (2);
- AdjACW = (11);
- gain = (0.0001);
- }
- else if(rpm > 245 and rpm < 248)
- {
- AdjCW = (2);
- AdjACW = (10);
- gain = (0.0001);
- }
- else if(rpm > 248 and rpm < 291)
- {
- AdjCW = (3);
- AdjACW = (10);
- gain = (0.0001);
- }
- else if(rpm > 291 and rpm < 322)
- {
- AdjCW = (3);
- AdjACW = (9);
- gain = (0.0001);
- }
- else if(rpm > 322 and rpm < 328)
- {
- AdjCW = (3);
- AdjACW = (8);
- gain = (0.0001);
- }
- else if(rpm > 328 and rpm < 394)
- {
- AdjCW = (4);
- AdjACW = (8);
- gain = (0.0001);
- }
- else if(rpm > 394 and rpm < 435)
- {
- AdjCW = (4);
- AdjACW = (7);
- gain = (0.0001);
- }
- else if(rpm > 435 and rpm < 459)
- {
- AdjCW = (5);
- AdjACW = (7);
- gain = (0.0001);
- }
- else if(rpm > 459 and rpm < 512 )
- {
- AdjCW = (5);
- AdjACW = (6);
- gain = (0.0001);
- }
- else if(rpm > 512 and rpm < 533)
- {
- AdjCW = (6);
- AdjACW = (6);
- gain = (0.0001);
- }
- else if(rpm > 533 and rpm < 641)
- {
- AdjCW = (6);
- AdjACW = (5);
- gain = (0.0001);
- }
- else if(rpm > 641 and rpm < 763)
- {
- AdjCW = (7);
- AdjACW = (5);
- gain = (0.0001);
- }
- else if(rpm > 763 and rpm < 1002)
- {
- AdjCW = (7);
- AdjACW = (4);
- gain = (0.0001);
- }
- else if(rpm > 1002 and rpm < 1318)
- {
- AdjCW = (8);
- AdjACW = (4);
- gain = (0.00001);
- }
- else if(rpm > 1318 and rpm < 1338)
- {
- AdjCW = (8);
- AdjACW = (3);
- gain = (0.00001);
- }
- else if(rpm > 1338 and rpm < 1463)
- {
- AdjCW = (9);
- AdjACW = (3);
- gain = (0.00001);
- }
- else if(rpm > 1463 and rpm < 1493)
- {
- AdjCW = (9);
- AdjACW = (2);
- gain = (0.00001);
- }
- else if(rpm > 1493 and rpm < 1719)
- {
- AdjCW = (10);
- AdjACW = (2);
- gain = (0.00001);
- }
- else if(rpm > 1719 and rpm < 1780)
- {
- AdjCW = (11);
- AdjACW = (2);
- gain = (0.00001);
- }
- else if(rpm > 1780 and rpm < 2030)
- {
- AdjCW = (11);
- AdjACW = (1);
- gain = (0.00001);
- }
- else if(rpm > 2030 and rpm < 2073)
- {
- AdjCW = (12);
- AdjACW = (1);
- gain = (0.00001);
- }
- else if(rpm > 2073 and rpm < 2255)
- {
- AdjCW = (12);
- AdjACW = (0);
- gain = (0.00001);
- }
- else if(rpm > 2255 and rpm < 2463)
- {
- AdjCW = (13);
- AdjACW = (0);
- gain = (0.00001);
- }
- else if(rpm > 2463 and rpm < 2479)
- {
- AdjCW = (14);
- AdjACW = (0);
- gain = (0.00001);
- }
- else if(rpm > 2479 and rpm < 2591)
- {
- AdjCW = (14);
- AdjACW = (63);
- gain = (0.00001);
- }
- else if(rpm > 2591 and rpm < 2612)
- {
- AdjCW = (14);
- AdjACW = (62);
- gain = (0.00001);
- }
- else if(rpm > 2612 and rpm < 2742)
- {
- AdjCW = (15);
- AdjACW = (62);
- gain = (0.00001);
- }
- else if(rpm > 2742 and rpm < 2762)
- {
- AdjCW = (16);
- AdjACW = (62);
- gain = (0.00001);
- }
- else if(rpm > 2762 and rpm < 2894)
- {
- AdjCW = (16);
- AdjACW = (61);
- gain = (0.00001);
- }
- else if(rpm > 2894 and rpm < 2926)
- {
- AdjCW = (17);
- AdjACW = (61);
- gain = (0.000001);
- }
- else if(rpm > 2926 and rpm < 2982)
- {
- AdjCW = (17);
- AdjACW = (59);
- gain = (0.000001);
- }
- else if(rpm > 2982)
- {
- AdjCW = (17);
- AdjACW = (57);
- gain = (0.000001);
- } */
- /*}
-
- if(duty > 0.99) //Mapped Phase Advance with load
- {
- if(rpm < 48)
- {
- AdjCW = (52);
- AdjACW = (19);
- gain = (0.0001);
- }
- else if(rpm > 48 and rpm < 56)
- {
- AdjCW = (53);
- AdjACW = (19);
- gain = (0.0001);
- }
- else if(rpm > 56 and rpm < 76)
- {
- AdjCW = (55);
- AdjACW = (19);
- gain = (0.0001);
- }
- else if(rpm > 76 and rpm < 90)
- {
- AdjCW = (58);
- AdjACW = (19);
- gain = (0.0001);
- }
- else if(rpm > 90 and rpm < 125)
- {
- AdjCW = (58);
- AdjACW = (14);
- gain = (0.0001);
- }
- else if(rpm > 125 and rpm < 140)
- {
- AdjCW = (62);
- AdjACW = (14);
- gain = (0.0001);
- }
- else if(rpm > 140 and rpm < 190)
- {
- AdjCW = (62);
- AdjACW = (9);
- gain = (0.0001);
- }
- else if(rpm > 190 and rpm < 200)
- {
- AdjCW = (3);
- AdjACW = (9);
- gain = (0.0001);
- }
- else if(rpm > 200 and rpm < 350)
- {
- AdjCW = (3);
- AdjACW = (9);
- gain = (0.0001);
- }
- else if(rpm > 350 and rpm < 650)
- {
- AdjCW = (3);
- AdjACW = (5);
- gain = (0.0001);
- }
- else if(rpm > 650)
- {
- AdjCW = (6);
- AdjACW = (5);
- gain = (0.000001);
- }
- }*/
-}
-
-void VelocityLoop (void) //Speed control
-{
- diff = SetPoint - rpm; //difference between setpoint and the RPM measurement
- duty = duty + (diff*gain); //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.11. 1 = max
- {
- duty = 1;
- }
- if (duty <0.053) //min duty 0.11
- {
- duty = 0.053;
- }
-}
-
-void ReadKType(void) //Reads Temperature
-{
- int i = 0;
- int Readout = 0;
-
- cs1 = 0;
- SerialClock = 0; //set clock to 0
- wait_ms(0.01);
- SerialClock = 1; //clock once to set to the 13 bit temp data
- wait_ms(0.01);
- SerialClock = 0;
- 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
- {
- 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.01);
- SerialClock = 0;
- wait_ms(0.01);
- }
- temp = Readout * 0.125; //get the real temp value which is a float
- Readout = 0;
- cs1 = 1;
-}
-
-void Switch (void)
-{
- led1 = clockwise;
- led2 = anticlockwise;
}
\ No newline at end of file