Rotork Research Team / Mbed 2 deprecated 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.

Dependencies:   mbed QEI

main.cpp@21:b831f68ce5ed, 2019-04-18 (annotated)

Committer:
simontruelove
Date:
Thu Apr 18 08:15:22 2019 +0000
Revision:
21:b831f68ce5ed
Parent:
20:dca9f4c12fe3
Child:
22:58558001804d
auto gain adjust added

Who changed what in which revision?

UserRevisionLine numberNew contents of line
simontruelove 0:634dd505dace 1 #include "mbed.h"
simontruelove 3:4249dbdf7ed3 2 #include "QEI.h"
simontruelove 3:4249dbdf7ed3 3
simontruelove 20:dca9f4c12fe3 4 /* Code written by Simon Truelove (completed 5/4/2019) to control Switched Reluctance Motor (SRM) modified from a
simontruelove 20:dca9f4c12fe3 5 QSH6018-65-28-210 stepper motor. The position of the motor is monitored using a rotary encoder 102-1307-ND using QEI.h.
simontruelove 20:dca9f4c12fe3 6 The code uses the pulses from the encoder to increment the values StateA and StateB to control switching of the phases needed
simontruelove 20:dca9f4c12fe3 7 to generate rotation via a 64 case state machine. Upon start up the motor will step anti clockwise by switching each phase
simontruelove 20:dca9f4c12fe3 8 on in sequence then it will complete 2 revolutions to find the encoder index and set StateA which is used for calculating
simontruelove 20:dca9f4c12fe3 9 StateB. StateB controls 4 identical state machines, however state B is calculated slightly differently within each one
simontruelove 20:dca9f4c12fe3 10 depending on the direction of rotation required and the direction the motor had turned previously. LED's are switched on and
simontruelove 20:dca9f4c12fe3 11 off and these LED's control which of the 4 state machines is used.
simontruelove 20:dca9f4c12fe3 12 "Its flow"
simontruelove 20:dca9f4c12fe3 13 */
simontruelove 20:dca9f4c12fe3 14
simontruelove 20:dca9f4c12fe3 15 void Initialisation (void); //These function prototypes are written after the main. They must be listed here too
simontruelove 16:e6c8df9960c6 16 void StepACW(void);
simontruelove 13:da9d3fbbe407 17 void Ph0(void);
simontruelove 1:0191658b6ff4 18 void Ph1(void);
simontruelove 3:4249dbdf7ed3 19 void Ph12 (void);
simontruelove 1:0191658b6ff4 20 void Ph2(void);
simontruelove 3:4249dbdf7ed3 21 void Ph23 (void);
simontruelove 1:0191658b6ff4 22 void Ph3(void);
simontruelove 3:4249dbdf7ed3 23 void Ph34 (void);
simontruelove 1:0191658b6ff4 24 void Ph4(void);
simontruelove 3:4249dbdf7ed3 25 void Ph41 (void);
simontruelove 4:3aedc9246ae4 26 void GetChar (void);
simontruelove 5:4e5c644d5cc3 27 void RPM (void);
simontruelove 7:b8de1529c7fc 28 void VelocityLoop (void);
simontruelove 11:74eeb8871fe6 29 void ReadKType(void);
simontruelove 1:0191658b6ff4 30
simontruelove 10:808cb9052f14 31 Serial pc(USBTX, USBRX); // tx, rx - set up the Terraterm input from mbed
simontruelove 1:0191658b6ff4 32
simontruelove 10:808cb9052f14 33 QEI wheel(p5, p6, p8, 800, QEI::X4_ENCODING); //code for quadrature encoder see QEI.h
simontruelove 3:4249dbdf7ed3 34
simontruelove 10:808cb9052f14 35 Timer t; //timer used in RPM
simontruelove 3:4249dbdf7ed3 36
simontruelove 20:dca9f4c12fe3 37 DigitalOut led1 (LED1); //LEDs used to as very basic memmory for controlling the state machines
simontruelove 20:dca9f4c12fe3 38 DigitalOut led2 (LED2);
simontruelove 20:dca9f4c12fe3 39 DigitalOut led3 (LED3);
simontruelove 20:dca9f4c12fe3 40 DigitalOut led4 (LED4);
simontruelove 20:dca9f4c12fe3 41 DigitalOut SerialClock (p12); //ReadKType SPI
simontruelove 20:dca9f4c12fe3 42 DigitalIn DOut (p13); //ReadKType SPI
simontruelove 20:dca9f4c12fe3 43 DigitalOut cs1 (p14); //Chip Select SPI
simontruelove 20:dca9f4c12fe3 44 DigitalOut UnUsedPhase1 (p21);
simontruelove 20:dca9f4c12fe3 45 PwmOut Phase1 (p22); //Pin set up - PWM to enable speed control
simontruelove 20:dca9f4c12fe3 46 PwmOut Phase2 (p23);
simontruelove 20:dca9f4c12fe3 47 PwmOut Phase3 (p24);
simontruelove 20:dca9f4c12fe3 48 PwmOut Phase4 (p25);
simontruelove 20:dca9f4c12fe3 49 DigitalOut UnUsedPhase2 (p26);
simontruelove 10:808cb9052f14 50
simontruelove 10:808cb9052f14 51 int StateA = 0; //State for first 2 revolutions (calibration of the index)
simontruelove 10:808cb9052f14 52 int StateB = 0; //All state machines after calibration use this state
simontruelove 20:dca9f4c12fe3 53 int AdjCW = 57; //CW offset to adjust phase firing to give the fastest speed = used to calculate stateB
simontruelove 20:dca9f4c12fe3 54 int AdjACW = 12; //ACW offset to adjust phase firing to give the fastest speed = used to calculate stateB
simontruelove 20:dca9f4c12fe3 55 int CW = 57; //Value used to start motor from stationary
simontruelove 20:dca9f4c12fe3 56 int ACW = 12; //Value used to start motor from stationary
simontruelove 10:808cb9052f14 57 int TimePerClick = 0; //for calc of RPM
simontruelove 21:b831f68ce5ed 58 int SetPoint = 1500; //for adjusting the speed
simontruelove 17:19b2c598810a 59 int enc = 3200; //800 x4 enc = 3200 Pulses Per Rev
simontruelove 20:dca9f4c12fe3 60 int s = enc/50; //s=number of cases in state machine (64)
simontruelove 20:dca9f4c12fe3 61 int T = 80; //Thermostat protection limit degrees centigrade
simontruelove 20:dca9f4c12fe3 62 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
simontruelove 20:dca9f4c12fe3 63 int slowloop = 0; //Counter used to read temp at a slower rate
simontruelove 11:74eeb8871fe6 64
simontruelove 10:808cb9052f14 65 char c; //keyboard cotrol GetChar
simontruelove 4:3aedc9246ae4 66
simontruelove 20:dca9f4c12fe3 67 float rps = 0; //for calc of RPM
simontruelove 20:dca9f4c12fe3 68 float rpm = 0; //for calc of RPM
simontruelove 17:19b2c598810a 69 float duty = 1;
simontruelove 10:808cb9052f14 70 float diff = 0.0; //Velocity loop: diff = SetPoint - rpm;
simontruelove 21:b831f68ce5ed 71 float gain = 0.0001; //Velocity loop
simontruelove 20:dca9f4c12fe3 72 float p = 0.000014; //PWM period
simontruelove 20:dca9f4c12fe3 73 float x=0.1; //Used in StepACW x=time of square wave when 1 phase energised,
simontruelove 20:dca9f4c12fe3 74 float TimePerRev = 0; //for calc of RPM
simontruelove 20:dca9f4c12fe3 75 float y=0.04; //Used in Step ACWy=time of square wave when 2 phases energised
simontruelove 10:808cb9052f14 76 float temp = 0; //ReadKType
simontruelove 1:0191658b6ff4 77
simontruelove 1:0191658b6ff4 78 int main(void)
simontruelove 3:4249dbdf7ed3 79 {
simontruelove 17:19b2c598810a 80 pc.baud(921600); //Set fastest baud rate
simontruelove 20:dca9f4c12fe3 81 Phase1.period(p); //period of 0.000002 = 2 microseconds (500kHz). Good balance of low and high speed performance.
simontruelove 16:e6c8df9960c6 82 Phase2.period(p);
simontruelove 16:e6c8df9960c6 83 Phase3.period(p);
simontruelove 16:e6c8df9960c6 84 Phase4.period(p);
simontruelove 14:1eb49362a607 85 wait(0.1);
simontruelove 14:1eb49362a607 86 t.start();
simontruelove 14:1eb49362a607 87 SerialClock = 0;
simontruelove 16:e6c8df9960c6 88 StepACW();
simontruelove 3:4249dbdf7ed3 89 Initialisation();
simontruelove 14:1eb49362a607 90
simontruelove 20:dca9f4c12fe3 91 while(wheel.getRevolutions()<2) //Index Calibration
simontruelove 20:dca9f4c12fe3 92 {
simontruelove 14:1eb49362a607 93 StateA = (wheel.getPulses()+25)%s;
simontruelove 5:4e5c644d5cc3 94 switch(StateA)
simontruelove 3:4249dbdf7ed3 95 {
simontruelove 20:dca9f4c12fe3 96 case 0:Ph1();break;
simontruelove 6:f7028034aabb 97 case 1:Ph1();break;
simontruelove 14:1eb49362a607 98 case 2:Ph1();break;
simontruelove 14:1eb49362a607 99 case 3:Ph1();break;
simontruelove 14:1eb49362a607 100 case 4:Ph1();break;
simontruelove 14:1eb49362a607 101 case 5:Ph1();break;
simontruelove 14:1eb49362a607 102 case 6:Ph1();break;
simontruelove 14:1eb49362a607 103 case 7:Ph1();break;
simontruelove 14:1eb49362a607 104 case 8:Ph1();break;
simontruelove 14:1eb49362a607 105 case 9:Ph1();break;
simontruelove 14:1eb49362a607 106 case 10:Ph1();break;
simontruelove 14:1eb49362a607 107 case 11:Ph1();break;
simontruelove 14:1eb49362a607 108 case 12:Ph1();break;
simontruelove 14:1eb49362a607 109 case 13:Ph1();break;
simontruelove 14:1eb49362a607 110 case 14:Ph1();break;
simontruelove 14:1eb49362a607 111 case 15:Ph1();break;
simontruelove 20:dca9f4c12fe3 112 case 16:Ph2();break;
simontruelove 14:1eb49362a607 113 case 17:Ph2();break;
simontruelove 14:1eb49362a607 114 case 18:Ph2();break;
simontruelove 14:1eb49362a607 115 case 19:Ph2();break;
simontruelove 14:1eb49362a607 116 case 20:Ph2();break;
simontruelove 14:1eb49362a607 117 case 21:Ph2();break;
simontruelove 14:1eb49362a607 118 case 22:Ph2();break;
simontruelove 14:1eb49362a607 119 case 23:Ph2();break;
simontruelove 14:1eb49362a607 120 case 24:Ph2();break;
simontruelove 14:1eb49362a607 121 case 25:Ph2();break;
simontruelove 14:1eb49362a607 122 case 26:Ph2();break;
simontruelove 14:1eb49362a607 123 case 27:Ph2();break;
simontruelove 14:1eb49362a607 124 case 28:Ph2();break;
simontruelove 14:1eb49362a607 125 case 29:Ph2();break;
simontruelove 14:1eb49362a607 126 case 30:Ph2();break;
simontruelove 14:1eb49362a607 127 case 31:Ph2();break;
simontruelove 20:dca9f4c12fe3 128 case 32:Ph3();break;
simontruelove 14:1eb49362a607 129 case 33:Ph3();break;
simontruelove 14:1eb49362a607 130 case 34:Ph3();break;
simontruelove 14:1eb49362a607 131 case 35:Ph3();break;
simontruelove 14:1eb49362a607 132 case 36:Ph3();break;
simontruelove 14:1eb49362a607 133 case 37:Ph3();break;
simontruelove 14:1eb49362a607 134 case 38:Ph3();break;
simontruelove 14:1eb49362a607 135 case 39:Ph3();break;
simontruelove 14:1eb49362a607 136 case 40:Ph3();break;
simontruelove 14:1eb49362a607 137 case 41:Ph3();break;
simontruelove 14:1eb49362a607 138 case 42:Ph3();break;
simontruelove 14:1eb49362a607 139 case 43:Ph3();break;
simontruelove 14:1eb49362a607 140 case 44:Ph3();break;
simontruelove 14:1eb49362a607 141 case 45:Ph3();break;
simontruelove 14:1eb49362a607 142 case 46:Ph3();break;
simontruelove 14:1eb49362a607 143 case 47:Ph3();break;
simontruelove 20:dca9f4c12fe3 144 case 48:Ph4();break;
simontruelove 14:1eb49362a607 145 case 49:Ph4();break;
simontruelove 14:1eb49362a607 146 case 50:Ph4();break;
simontruelove 14:1eb49362a607 147 case 51:Ph4();break;
simontruelove 14:1eb49362a607 148 case 52:Ph4();break;
simontruelove 14:1eb49362a607 149 case 53:Ph4();break;
simontruelove 14:1eb49362a607 150 case 54:Ph4();break;
simontruelove 14:1eb49362a607 151 case 55:Ph4();break;
simontruelove 14:1eb49362a607 152 case 56:Ph4();break;
simontruelove 14:1eb49362a607 153 case 57:Ph4();break;
simontruelove 14:1eb49362a607 154 case 58:Ph4();break;
simontruelove 14:1eb49362a607 155 case 59:Ph4();break;
simontruelove 14:1eb49362a607 156 case 60:Ph4();break;
simontruelove 14:1eb49362a607 157 case 61:Ph4();break;
simontruelove 14:1eb49362a607 158 case 62:Ph4();break;
simontruelove 14:1eb49362a607 159 case 63:Ph4();break;
simontruelove 5:4e5c644d5cc3 160 default:break;
simontruelove 3:4249dbdf7ed3 161 }
simontruelove 20:dca9f4c12fe3 162 if(wheel.getYay()==1) //PulseCount_==1, yay_=1 used to increment the StateA count
simontruelove 3:4249dbdf7ed3 163 {
simontruelove 5:4e5c644d5cc3 164 StateA++;
simontruelove 5:4e5c644d5cc3 165 wheel.ResetYay();
simontruelove 14:1eb49362a607 166 if (StateA>(s-1))
simontruelove 3:4249dbdf7ed3 167 {
simontruelove 5:4e5c644d5cc3 168 StateA=0;
simontruelove 3:4249dbdf7ed3 169 }
simontruelove 3:4249dbdf7ed3 170 }
simontruelove 14:1eb49362a607 171 }
simontruelove 3:4249dbdf7ed3 172
simontruelove 5:4e5c644d5cc3 173 while(1)
simontruelove 5:4e5c644d5cc3 174 {
simontruelove 10:808cb9052f14 175 while((led1 == 0) && (led2 == 0)) //If no command to operate
simontruelove 4:3aedc9246ae4 176 {
simontruelove 20:dca9f4c12fe3 177 duty = 0.3; //Duty reduced to low value to ensure ramp up of speed
simontruelove 20:dca9f4c12fe3 178 rpm = 0; //RPM function not being triggered due to no rotation. RPM set to 0
simontruelove 20:dca9f4c12fe3 179 AdjCW = CW; //Reset to correct value for start up
simontruelove 20:dca9f4c12fe3 180 AdjACW = ACW; //Reset to correct value for start up
simontruelove 20:dca9f4c12fe3 181 Ph0(); //turn off all phases
simontruelove 20:dca9f4c12fe3 182 GetChar(); //read keyboard strikes
simontruelove 20:dca9f4c12fe3 183 ReadKType(); //Temperature measurement
simontruelove 20:dca9f4c12fe3 184 StateB = (wheel.getPulses()+StateA)%s; //calculation for stateB
simontruelove 4:3aedc9246ae4 185 }
simontruelove 11:74eeb8871fe6 186 while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led1==1) && (temp<T)) //After Calibration, Prev CW movement, CW command
simontruelove 3:4249dbdf7ed3 187 {
simontruelove 20:dca9f4c12fe3 188 GetChar(); //read keyboard strikes
simontruelove 20:dca9f4c12fe3 189 StateB = (wheel.getPulses()+StateA+AdjCW)%s; //calculation for stateB
simontruelove 4:3aedc9246ae4 190
simontruelove 4:3aedc9246ae4 191 switch(StateB)
simontruelove 4:3aedc9246ae4 192 {
simontruelove 6:f7028034aabb 193 case 0:Ph1();break;
simontruelove 6:f7028034aabb 194 case 1:Ph1();break;
simontruelove 13:da9d3fbbe407 195 case 2:Ph1();break;
simontruelove 13:da9d3fbbe407 196 case 3:Ph1();break;
simontruelove 14:1eb49362a607 197 case 4:Ph1();break;
simontruelove 14:1eb49362a607 198 case 5:Ph1();break;
simontruelove 14:1eb49362a607 199 case 6:Ph1();break;
simontruelove 14:1eb49362a607 200 case 7:Ph1();break;
simontruelove 14:1eb49362a607 201 case 8:Ph1();break;
simontruelove 14:1eb49362a607 202 case 9:Ph1();break;
simontruelove 16:e6c8df9960c6 203 case 10:Ph0();break;
simontruelove 14:1eb49362a607 204 case 11:Ph1();break;
simontruelove 14:1eb49362a607 205 case 12:Ph1();break;
simontruelove 16:e6c8df9960c6 206 case 13:Ph0();break;
simontruelove 14:1eb49362a607 207 case 14:Ph1();break;
simontruelove 16:e6c8df9960c6 208 case 15:Ph0();break;
simontruelove 14:1eb49362a607 209 case 16:Ph2();break;
simontruelove 14:1eb49362a607 210 case 17:Ph2();break;
simontruelove 14:1eb49362a607 211 case 18:Ph2();break;
simontruelove 14:1eb49362a607 212 case 19:Ph2();break;
simontruelove 14:1eb49362a607 213 case 20:Ph2();break;
simontruelove 14:1eb49362a607 214 case 21:Ph2();break;
simontruelove 14:1eb49362a607 215 case 22:Ph2();break;
simontruelove 14:1eb49362a607 216 case 23:Ph2();break;
simontruelove 14:1eb49362a607 217 case 24:Ph2();break;
simontruelove 14:1eb49362a607 218 case 25:Ph2();break;
simontruelove 16:e6c8df9960c6 219 case 26:Ph0();break;
simontruelove 14:1eb49362a607 220 case 27:Ph2();break;
simontruelove 14:1eb49362a607 221 case 28:Ph2();break;
simontruelove 16:e6c8df9960c6 222 case 29:Ph0();break;
simontruelove 14:1eb49362a607 223 case 30:Ph2();break;
simontruelove 16:e6c8df9960c6 224 case 31:Ph0();break;
simontruelove 14:1eb49362a607 225 case 32:Ph3();break;
simontruelove 14:1eb49362a607 226 case 33:Ph3();break;
simontruelove 14:1eb49362a607 227 case 34:Ph3();break;
simontruelove 14:1eb49362a607 228 case 35:Ph3();break;
simontruelove 14:1eb49362a607 229 case 36:Ph3();break;
simontruelove 14:1eb49362a607 230 case 37:Ph3();break;
simontruelove 14:1eb49362a607 231 case 38:Ph3();break;
simontruelove 14:1eb49362a607 232 case 39:Ph3();break;
simontruelove 14:1eb49362a607 233 case 40:Ph3();break;
simontruelove 14:1eb49362a607 234 case 41:Ph3();break;
simontruelove 16:e6c8df9960c6 235 case 42:Ph0();break;
simontruelove 14:1eb49362a607 236 case 43:Ph3();break;
simontruelove 14:1eb49362a607 237 case 44:Ph3();break;
simontruelove 16:e6c8df9960c6 238 case 45:Ph0();break;
simontruelove 14:1eb49362a607 239 case 46:Ph3();break;
simontruelove 16:e6c8df9960c6 240 case 47:Ph0();break;
simontruelove 14:1eb49362a607 241 case 48:Ph4();break;
simontruelove 14:1eb49362a607 242 case 49:Ph4();break;
simontruelove 14:1eb49362a607 243 case 50:Ph4();break;
simontruelove 14:1eb49362a607 244 case 51:Ph4();break;
simontruelove 14:1eb49362a607 245 case 52:Ph4();break;
simontruelove 14:1eb49362a607 246 case 53:Ph4();break;
simontruelove 14:1eb49362a607 247 case 54:Ph4();break;
simontruelove 14:1eb49362a607 248 case 55:Ph4();break;
simontruelove 14:1eb49362a607 249 case 56:Ph4();break;
simontruelove 14:1eb49362a607 250 case 57:Ph4();break;
simontruelove 16:e6c8df9960c6 251 case 58:Ph0();break;
simontruelove 14:1eb49362a607 252 case 59:Ph4();break;
simontruelove 14:1eb49362a607 253 case 60:Ph4();break;
simontruelove 16:e6c8df9960c6 254 case 61:Ph0();break;
simontruelove 14:1eb49362a607 255 case 62:Ph4();break;
simontruelove 16:e6c8df9960c6 256 case 63:Ph0();break;
simontruelove 16:e6c8df9960c6 257 default:break;
simontruelove 4:3aedc9246ae4 258 }
simontruelove 20:dca9f4c12fe3 259
simontruelove 20:dca9f4c12fe3 260 if(wheel.getWhoop()==1) //PulseCount2_==3200 (this is 1 rotation), whoop_=1;
simontruelove 4:3aedc9246ae4 261 {
simontruelove 20:dca9f4c12fe3 262 RPM(); //calculate RPM
simontruelove 20:dca9f4c12fe3 263 VelocityLoop(); //Adjust velocity
simontruelove 20:dca9f4c12fe3 264 slowloop++; //increment slowloop
simontruelove 20:dca9f4c12fe3 265 if(slowloop>(0.01*rpm)) //Reads temperature at a constant time interval
simontruelove 14:1eb49362a607 266 {
simontruelove 20:dca9f4c12fe3 267 ReadKType(); //Read Temp
simontruelove 20:dca9f4c12fe3 268 slowloop=0; //Reset slowloop
simontruelove 14:1eb49362a607 269 }
simontruelove 11:74eeb8871fe6 270 }
simontruelove 4:3aedc9246ae4 271 }
simontruelove 4:3aedc9246ae4 272
simontruelove 11:74eeb8871fe6 273 while(wheel.getRevolutions()>1 && wheel.getPulses()<1 && (led1==1) && (temp<T)) //After Calibration, Prev ACW movement, CW command
simontruelove 4:3aedc9246ae4 274 {
simontruelove 4:3aedc9246ae4 275 GetChar();
simontruelove 14:1eb49362a607 276 StateB = (enc+wheel.getPulses()+StateA+AdjCW)%s;
simontruelove 20:dca9f4c12fe3 277
simontruelove 3:4249dbdf7ed3 278 switch(StateB)
simontruelove 3:4249dbdf7ed3 279 {
simontruelove 6:f7028034aabb 280 case 0:Ph1();break;
simontruelove 6:f7028034aabb 281 case 1:Ph1();break;
simontruelove 13:da9d3fbbe407 282 case 2:Ph1();break;
simontruelove 13:da9d3fbbe407 283 case 3:Ph1();break;
simontruelove 14:1eb49362a607 284 case 4:Ph1();break;
simontruelove 14:1eb49362a607 285 case 5:Ph1();break;
simontruelove 14:1eb49362a607 286 case 6:Ph1();break;
simontruelove 14:1eb49362a607 287 case 7:Ph1();break;
simontruelove 14:1eb49362a607 288 case 8:Ph1();break;
simontruelove 14:1eb49362a607 289 case 9:Ph1();break;
simontruelove 16:e6c8df9960c6 290 case 10:Ph0();break;
simontruelove 14:1eb49362a607 291 case 11:Ph1();break;
simontruelove 14:1eb49362a607 292 case 12:Ph1();break;
simontruelove 16:e6c8df9960c6 293 case 13:Ph0();break;
simontruelove 14:1eb49362a607 294 case 14:Ph1();break;
simontruelove 16:e6c8df9960c6 295 case 15:Ph0();break;
simontruelove 14:1eb49362a607 296 case 16:Ph2();break;
simontruelove 14:1eb49362a607 297 case 17:Ph2();break;
simontruelove 14:1eb49362a607 298 case 18:Ph2();break;
simontruelove 14:1eb49362a607 299 case 19:Ph2();break;
simontruelove 14:1eb49362a607 300 case 20:Ph2();break;
simontruelove 14:1eb49362a607 301 case 21:Ph2();break;
simontruelove 14:1eb49362a607 302 case 22:Ph2();break;
simontruelove 14:1eb49362a607 303 case 23:Ph2();break;
simontruelove 14:1eb49362a607 304 case 24:Ph2();break;
simontruelove 14:1eb49362a607 305 case 25:Ph2();break;
simontruelove 16:e6c8df9960c6 306 case 26:Ph0();break;
simontruelove 14:1eb49362a607 307 case 27:Ph2();break;
simontruelove 14:1eb49362a607 308 case 28:Ph2();break;
simontruelove 16:e6c8df9960c6 309 case 29:Ph0();break;
simontruelove 14:1eb49362a607 310 case 30:Ph2();break;
simontruelove 16:e6c8df9960c6 311 case 31:Ph0();break;
simontruelove 14:1eb49362a607 312 case 32:Ph3();break;
simontruelove 14:1eb49362a607 313 case 33:Ph3();break;
simontruelove 14:1eb49362a607 314 case 34:Ph3();break;
simontruelove 14:1eb49362a607 315 case 35:Ph3();break;
simontruelove 14:1eb49362a607 316 case 36:Ph3();break;
simontruelove 14:1eb49362a607 317 case 37:Ph3();break;
simontruelove 14:1eb49362a607 318 case 38:Ph3();break;
simontruelove 14:1eb49362a607 319 case 39:Ph3();break;
simontruelove 14:1eb49362a607 320 case 40:Ph3();break;
simontruelove 14:1eb49362a607 321 case 41:Ph3();break;
simontruelove 16:e6c8df9960c6 322 case 42:Ph0();break;
simontruelove 14:1eb49362a607 323 case 43:Ph3();break;
simontruelove 14:1eb49362a607 324 case 44:Ph3();break;
simontruelove 16:e6c8df9960c6 325 case 45:Ph0();break;
simontruelove 14:1eb49362a607 326 case 46:Ph3();break;
simontruelove 16:e6c8df9960c6 327 case 47:Ph0();break;
simontruelove 14:1eb49362a607 328 case 48:Ph4();break;
simontruelove 14:1eb49362a607 329 case 49:Ph4();break;
simontruelove 14:1eb49362a607 330 case 50:Ph4();break;
simontruelove 14:1eb49362a607 331 case 51:Ph4();break;
simontruelove 14:1eb49362a607 332 case 52:Ph4();break;
simontruelove 14:1eb49362a607 333 case 53:Ph4();break;
simontruelove 14:1eb49362a607 334 case 54:Ph4();break;
simontruelove 14:1eb49362a607 335 case 55:Ph4();break;
simontruelove 14:1eb49362a607 336 case 56:Ph4();break;
simontruelove 14:1eb49362a607 337 case 57:Ph4();break;
simontruelove 16:e6c8df9960c6 338 case 58:Ph0();break;
simontruelove 14:1eb49362a607 339 case 59:Ph4();break;
simontruelove 14:1eb49362a607 340 case 60:Ph4();break;
simontruelove 16:e6c8df9960c6 341 case 61:Ph0();break;
simontruelove 14:1eb49362a607 342 case 62:Ph4();break;
simontruelove 16:e6c8df9960c6 343 case 63:Ph0();break;
simontruelove 5:4e5c644d5cc3 344 default:break;
simontruelove 3:4249dbdf7ed3 345 }
simontruelove 20:dca9f4c12fe3 346
simontruelove 20:dca9f4c12fe3 347 if(wheel.getWhoop()==1)
simontruelove 5:4e5c644d5cc3 348 {
simontruelove 5:4e5c644d5cc3 349 RPM();
simontruelove 9:061600a6c750 350 VelocityLoop();
simontruelove 14:1eb49362a607 351 slowloop++;
simontruelove 16:e6c8df9960c6 352 if(slowloop>(0.01*rpm))
simontruelove 14:1eb49362a607 353 {
simontruelove 14:1eb49362a607 354 ReadKType();
simontruelove 14:1eb49362a607 355 slowloop=0;
simontruelove 14:1eb49362a607 356 }
simontruelove 11:74eeb8871fe6 357 }
simontruelove 5:4e5c644d5cc3 358 }
simontruelove 11:74eeb8871fe6 359 while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led2==1) && (temp<T)) //After Calibration, Prev CW movement, ACW command
simontruelove 4:3aedc9246ae4 360 {
simontruelove 5:4e5c644d5cc3 361 GetChar();
simontruelove 14:1eb49362a607 362 StateB = (wheel.getPulses()+StateA+AdjACW)%s;
simontruelove 20:dca9f4c12fe3 363
simontruelove 5:4e5c644d5cc3 364 switch(StateB)
simontruelove 5:4e5c644d5cc3 365 {
simontruelove 14:1eb49362a607 366 case 63:Ph4();break;
simontruelove 14:1eb49362a607 367 case 62:Ph4();break;
simontruelove 14:1eb49362a607 368 case 61:Ph4();break;
simontruelove 14:1eb49362a607 369 case 60:Ph4();break;
simontruelove 14:1eb49362a607 370 case 59:Ph4();break;
simontruelove 14:1eb49362a607 371 case 58:Ph4();break;
simontruelove 14:1eb49362a607 372 case 57:Ph4();break;
simontruelove 14:1eb49362a607 373 case 56:Ph4();break;
simontruelove 14:1eb49362a607 374 case 55:Ph4();break;
simontruelove 14:1eb49362a607 375 case 54:Ph4();break;
simontruelove 16:e6c8df9960c6 376 case 53:Ph0();break;
simontruelove 14:1eb49362a607 377 case 52:Ph4();break;
simontruelove 14:1eb49362a607 378 case 51:Ph4();break;
simontruelove 16:e6c8df9960c6 379 case 50:Ph0();break;
simontruelove 14:1eb49362a607 380 case 49:Ph4();break;
simontruelove 16:e6c8df9960c6 381 case 48:Ph0();break;
simontruelove 14:1eb49362a607 382 case 47:Ph3();break;
simontruelove 14:1eb49362a607 383 case 46:Ph3();break;
simontruelove 14:1eb49362a607 384 case 45:Ph3();break;
simontruelove 14:1eb49362a607 385 case 44:Ph3();break;
simontruelove 14:1eb49362a607 386 case 43:Ph3();break;
simontruelove 14:1eb49362a607 387 case 42:Ph3();break;
simontruelove 14:1eb49362a607 388 case 41:Ph3();break;
simontruelove 14:1eb49362a607 389 case 40:Ph3();break;
simontruelove 14:1eb49362a607 390 case 39:Ph3();break;
simontruelove 14:1eb49362a607 391 case 38:Ph3();break;
simontruelove 16:e6c8df9960c6 392 case 37:Ph0();break;
simontruelove 14:1eb49362a607 393 case 36:Ph3();break;
simontruelove 14:1eb49362a607 394 case 35:Ph3();break;
simontruelove 16:e6c8df9960c6 395 case 34:Ph0();break;
simontruelove 14:1eb49362a607 396 case 33:Ph3();break;
simontruelove 16:e6c8df9960c6 397 case 32:Ph0();break;
simontruelove 14:1eb49362a607 398 case 31:Ph2();break;
simontruelove 14:1eb49362a607 399 case 30:Ph2();break;
simontruelove 14:1eb49362a607 400 case 29:Ph2();break;
simontruelove 14:1eb49362a607 401 case 28:Ph2();break;
simontruelove 14:1eb49362a607 402 case 27:Ph2();break;
simontruelove 14:1eb49362a607 403 case 26:Ph2();break;
simontruelove 14:1eb49362a607 404 case 25:Ph2();break;
simontruelove 14:1eb49362a607 405 case 24:Ph2();break;
simontruelove 14:1eb49362a607 406 case 23:Ph2();break;
simontruelove 14:1eb49362a607 407 case 22:Ph2();break;
simontruelove 16:e6c8df9960c6 408 case 21:Ph0();break;
simontruelove 14:1eb49362a607 409 case 20:Ph2();break;
simontruelove 14:1eb49362a607 410 case 19:Ph2();break;
simontruelove 16:e6c8df9960c6 411 case 18:Ph0();break;
simontruelove 14:1eb49362a607 412 case 17:Ph2();break;
simontruelove 16:e6c8df9960c6 413 case 16:Ph0();break;
simontruelove 14:1eb49362a607 414 case 15:Ph1();break;
simontruelove 14:1eb49362a607 415 case 14:Ph1();break;
simontruelove 14:1eb49362a607 416 case 13:Ph1();break;
simontruelove 14:1eb49362a607 417 case 12:Ph1();break;
simontruelove 14:1eb49362a607 418 case 11:Ph1();break;
simontruelove 14:1eb49362a607 419 case 10:Ph1();break;
simontruelove 14:1eb49362a607 420 case 9:Ph1();break;
simontruelove 14:1eb49362a607 421 case 8:Ph1();break;
simontruelove 14:1eb49362a607 422 case 7:Ph1();break;
simontruelove 14:1eb49362a607 423 case 6:Ph1();break;
simontruelove 16:e6c8df9960c6 424 case 5:Ph0();break;
simontruelove 14:1eb49362a607 425 case 4:Ph1();break;
simontruelove 13:da9d3fbbe407 426 case 3:Ph1();break;
simontruelove 16:e6c8df9960c6 427 case 2:Ph0();break;
simontruelove 6:f7028034aabb 428 case 1:Ph1();break;
simontruelove 16:e6c8df9960c6 429 case 0:Ph0();break;
simontruelove 5:4e5c644d5cc3 430 default:break;
simontruelove 20:dca9f4c12fe3 431 }
simontruelove 20:dca9f4c12fe3 432 if(wheel.getWhoop()==1)
simontruelove 5:4e5c644d5cc3 433 {
simontruelove 5:4e5c644d5cc3 434 RPM();
simontruelove 9:061600a6c750 435 VelocityLoop();
simontruelove 14:1eb49362a607 436 slowloop++;
simontruelove 16:e6c8df9960c6 437 if(slowloop>(0.01*rpm))
simontruelove 14:1eb49362a607 438 {
simontruelove 14:1eb49362a607 439 ReadKType();
simontruelove 14:1eb49362a607 440 slowloop=0;
simontruelove 14:1eb49362a607 441 }
simontruelove 5:4e5c644d5cc3 442 }
simontruelove 3:4249dbdf7ed3 443 }
simontruelove 11:74eeb8871fe6 444 while((wheel.getRevolutions()>1) && (wheel.getPulses()<1) && (led2==1) &&(temp<T)) //After Calibration, Prev ACW movement, ACW command
simontruelove 5:4e5c644d5cc3 445 {
simontruelove 5:4e5c644d5cc3 446 GetChar();
simontruelove 14:1eb49362a607 447 StateB = (enc+wheel.getPulses()+StateA+AdjACW)%s;
simontruelove 20:dca9f4c12fe3 448
simontruelove 5:4e5c644d5cc3 449 switch(StateB)
simontruelove 5:4e5c644d5cc3 450 {
simontruelove 14:1eb49362a607 451 case 63:Ph4();break;
simontruelove 14:1eb49362a607 452 case 62:Ph4();break;
simontruelove 14:1eb49362a607 453 case 61:Ph4();break;
simontruelove 14:1eb49362a607 454 case 60:Ph4();break;
simontruelove 14:1eb49362a607 455 case 59:Ph4();break;
simontruelove 14:1eb49362a607 456 case 58:Ph4();break;
simontruelove 14:1eb49362a607 457 case 57:Ph4();break;
simontruelove 14:1eb49362a607 458 case 56:Ph4();break;
simontruelove 14:1eb49362a607 459 case 55:Ph4();break;
simontruelove 14:1eb49362a607 460 case 54:Ph4();break;
simontruelove 16:e6c8df9960c6 461 case 53:Ph0();break;
simontruelove 14:1eb49362a607 462 case 52:Ph4();break;
simontruelove 14:1eb49362a607 463 case 51:Ph4();break;
simontruelove 16:e6c8df9960c6 464 case 50:Ph0();break;
simontruelove 14:1eb49362a607 465 case 49:Ph4();break;
simontruelove 16:e6c8df9960c6 466 case 48:Ph0();break;
simontruelove 14:1eb49362a607 467 case 47:Ph3();break;
simontruelove 14:1eb49362a607 468 case 46:Ph3();break;
simontruelove 14:1eb49362a607 469 case 45:Ph3();break;
simontruelove 14:1eb49362a607 470 case 44:Ph3();break;
simontruelove 14:1eb49362a607 471 case 43:Ph3();break;
simontruelove 14:1eb49362a607 472 case 42:Ph3();break;
simontruelove 14:1eb49362a607 473 case 41:Ph3();break;
simontruelove 14:1eb49362a607 474 case 40:Ph3();break;
simontruelove 14:1eb49362a607 475 case 39:Ph3();break;
simontruelove 14:1eb49362a607 476 case 38:Ph3();break;
simontruelove 16:e6c8df9960c6 477 case 37:Ph0();break;
simontruelove 14:1eb49362a607 478 case 36:Ph3();break;
simontruelove 14:1eb49362a607 479 case 35:Ph3();break;
simontruelove 16:e6c8df9960c6 480 case 34:Ph0();break;
simontruelove 14:1eb49362a607 481 case 33:Ph3();break;
simontruelove 16:e6c8df9960c6 482 case 32:Ph0();break;
simontruelove 14:1eb49362a607 483 case 31:Ph2();break;
simontruelove 14:1eb49362a607 484 case 30:Ph2();break;
simontruelove 14:1eb49362a607 485 case 29:Ph2();break;
simontruelove 14:1eb49362a607 486 case 28:Ph2();break;
simontruelove 14:1eb49362a607 487 case 27:Ph2();break;
simontruelove 14:1eb49362a607 488 case 26:Ph2();break;
simontruelove 14:1eb49362a607 489 case 25:Ph2();break;
simontruelove 14:1eb49362a607 490 case 24:Ph2();break;
simontruelove 14:1eb49362a607 491 case 23:Ph2();break;
simontruelove 14:1eb49362a607 492 case 22:Ph2();break;
simontruelove 16:e6c8df9960c6 493 case 21:Ph0();break;
simontruelove 14:1eb49362a607 494 case 20:Ph2();break;
simontruelove 14:1eb49362a607 495 case 19:Ph2();break;
simontruelove 16:e6c8df9960c6 496 case 18:Ph0();break;
simontruelove 14:1eb49362a607 497 case 17:Ph2();break;
simontruelove 16:e6c8df9960c6 498 case 16:Ph0();break;
simontruelove 14:1eb49362a607 499 case 15:Ph1();break;
simontruelove 14:1eb49362a607 500 case 14:Ph1();break;
simontruelove 14:1eb49362a607 501 case 13:Ph1();break;
simontruelove 14:1eb49362a607 502 case 12:Ph1();break;
simontruelove 14:1eb49362a607 503 case 11:Ph1();break;
simontruelove 14:1eb49362a607 504 case 10:Ph1();break;
simontruelove 14:1eb49362a607 505 case 9:Ph1();break;
simontruelove 14:1eb49362a607 506 case 8:Ph1();break;
simontruelove 14:1eb49362a607 507 case 7:Ph1();break;
simontruelove 14:1eb49362a607 508 case 6:Ph1();break;
simontruelove 16:e6c8df9960c6 509 case 5:Ph0();break;
simontruelove 14:1eb49362a607 510 case 4:Ph1();break;
simontruelove 13:da9d3fbbe407 511 case 3:Ph1();break;
simontruelove 16:e6c8df9960c6 512 case 2:Ph0();break;
simontruelove 6:f7028034aabb 513 case 1:Ph1();break;
simontruelove 16:e6c8df9960c6 514 case 0:Ph0();break;
simontruelove 14:1eb49362a607 515 default:break;
simontruelove 5:4e5c644d5cc3 516 }
simontruelove 20:dca9f4c12fe3 517
simontruelove 20:dca9f4c12fe3 518 if(wheel.getWhoop()==1)
simontruelove 5:4e5c644d5cc3 519 {
simontruelove 5:4e5c644d5cc3 520 RPM();
simontruelove 9:061600a6c750 521 VelocityLoop();
simontruelove 14:1eb49362a607 522 slowloop++;
simontruelove 16:e6c8df9960c6 523 if(slowloop>(0.01*rpm))
simontruelove 14:1eb49362a607 524 {
simontruelove 14:1eb49362a607 525 ReadKType();
simontruelove 14:1eb49362a607 526 slowloop=0;
simontruelove 14:1eb49362a607 527 }
simontruelove 11:74eeb8871fe6 528 }
simontruelove 5:4e5c644d5cc3 529 }
simontruelove 20:dca9f4c12fe3 530 while(temp>(T-1)) //If Temp exceeds T the motor will not operate
simontruelove 11:74eeb8871fe6 531 {
simontruelove 11:74eeb8871fe6 532 Initialisation();
simontruelove 11:74eeb8871fe6 533 pc.printf("Motor Over Temp\n\r");
simontruelove 20:dca9f4c12fe3 534 while(1)
simontruelove 11:74eeb8871fe6 535 {
simontruelove 20:dca9f4c12fe3 536 ReadKType(); //Read motor Temp
simontruelove 20:dca9f4c12fe3 537 pc.printf("%f\r",temp); //Display current Temp
simontruelove 20:dca9f4c12fe3 538 wait(1);
simontruelove 20:dca9f4c12fe3 539 if(temp<T-20) //Reset when Temp 20 degrees C below thermostat trip value
simontruelove 20:dca9f4c12fe3 540 {
simontruelove 20:dca9f4c12fe3 541 pc.printf("Motor Back Online\n\r");
simontruelove 20:dca9f4c12fe3 542 break;
simontruelove 11:74eeb8871fe6 543 }
simontruelove 20:dca9f4c12fe3 544 }
simontruelove 11:74eeb8871fe6 545 }
simontruelove 1:0191658b6ff4 546 }
simontruelove 1:0191658b6ff4 547 }
simontruelove 16:e6c8df9960c6 548 void StepACW(void) //Square wave switching
simontruelove 1:0191658b6ff4 549 {
simontruelove 1:0191658b6ff4 550 Ph4();
simontruelove 3:4249dbdf7ed3 551 wait(x);
simontruelove 18:3493de6fe8ce 552 pc.printf("4 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 18:3493de6fe8ce 553 //Ph34();
simontruelove 18:3493de6fe8ce 554 //wait(y);
simontruelove 14:1eb49362a607 555 //pc.printf("34 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 556 Ph3();
simontruelove 14:1eb49362a607 557 wait(x);
simontruelove 18:3493de6fe8ce 558 pc.printf("3 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 18:3493de6fe8ce 559 //Ph23();
simontruelove 18:3493de6fe8ce 560 //wait(y);
simontruelove 14:1eb49362a607 561 //pc.printf("23 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 562 Ph2();
simontruelove 14:1eb49362a607 563 wait(x);
simontruelove 18:3493de6fe8ce 564 pc.printf("2 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 18:3493de6fe8ce 565 //Ph12();
simontruelove 18:3493de6fe8ce 566 //wait(y);
simontruelove 14:1eb49362a607 567 //pc.printf("12 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 568 Ph1();
simontruelove 14:1eb49362a607 569 wait(x);
simontruelove 18:3493de6fe8ce 570 pc.printf("1 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 18:3493de6fe8ce 571 //Ph41();
simontruelove 18:3493de6fe8ce 572 // wait(y);
simontruelove 14:1eb49362a607 573 //pc.printf("41 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 13:da9d3fbbe407 574 }
simontruelove 13:da9d3fbbe407 575
simontruelove 20:dca9f4c12fe3 576 void Ph0(void) //Turn off all Phases
simontruelove 13:da9d3fbbe407 577 {
simontruelove 13:da9d3fbbe407 578 Phase1.write(0);
simontruelove 13:da9d3fbbe407 579 Phase2.write(0);
simontruelove 13:da9d3fbbe407 580 Phase3.write(0);
simontruelove 13:da9d3fbbe407 581 Phase4.write(0);
simontruelove 1:0191658b6ff4 582 }
simontruelove 1:0191658b6ff4 583
simontruelove 20:dca9f4c12fe3 584 void Ph1(void) //Turn on Phase 1
simontruelove 1:0191658b6ff4 585 {
simontruelove 9:061600a6c750 586 Phase1.write(duty);
simontruelove 9:061600a6c750 587 Phase2.write(0);
simontruelove 9:061600a6c750 588 Phase3.write(0);
simontruelove 9:061600a6c750 589 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 590 }
simontruelove 3:4249dbdf7ed3 591
simontruelove 20:dca9f4c12fe3 592 void Ph12 (void) //Turn on Phase 1 and 2
simontruelove 3:4249dbdf7ed3 593 {
simontruelove 9:061600a6c750 594 Phase1.write(duty);
simontruelove 9:061600a6c750 595 Phase2.write(duty);
simontruelove 9:061600a6c750 596 Phase3.write(0);
simontruelove 9:061600a6c750 597 Phase4.write(0);
simontruelove 1:0191658b6ff4 598 }
simontruelove 1:0191658b6ff4 599
simontruelove 20:dca9f4c12fe3 600 void Ph2(void) //Turn on Phase 2
simontruelove 1:0191658b6ff4 601 {
simontruelove 9:061600a6c750 602 Phase1.write(0);
simontruelove 9:061600a6c750 603 Phase2.write(duty);
simontruelove 9:061600a6c750 604 Phase3.write(0);
simontruelove 9:061600a6c750 605 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 606 }
simontruelove 3:4249dbdf7ed3 607
simontruelove 20:dca9f4c12fe3 608 void Ph23 (void) //Turn on Phase 2 and 3
simontruelove 3:4249dbdf7ed3 609 {
simontruelove 9:061600a6c750 610 Phase1.write(0);
simontruelove 9:061600a6c750 611 Phase2.write(duty);
simontruelove 9:061600a6c750 612 Phase3.write(duty);
simontruelove 9:061600a6c750 613 Phase4.write(0);
simontruelove 1:0191658b6ff4 614 }
simontruelove 0:634dd505dace 615
simontruelove 20:dca9f4c12fe3 616 void Ph3(void) //Turn on Phase 3
simontruelove 1:0191658b6ff4 617 {
simontruelove 9:061600a6c750 618 Phase1.write(0);
simontruelove 9:061600a6c750 619 Phase2.write(0);
simontruelove 9:061600a6c750 620 Phase3.write(duty);
simontruelove 9:061600a6c750 621 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 622 }
simontruelove 20:dca9f4c12fe3 623
simontruelove 20:dca9f4c12fe3 624 void Ph34 (void) //Turn on Phase 3 and 4
simontruelove 3:4249dbdf7ed3 625 {
simontruelove 9:061600a6c750 626 Phase1.write(0);
simontruelove 9:061600a6c750 627 Phase2.write(0);
simontruelove 9:061600a6c750 628 Phase3.write(duty);
simontruelove 9:061600a6c750 629 Phase4.write(duty);
simontruelove 1:0191658b6ff4 630 }
simontruelove 1:0191658b6ff4 631
simontruelove 20:dca9f4c12fe3 632 void Ph4(void) //Turn on Phase 4
simontruelove 1:0191658b6ff4 633 {
simontruelove 9:061600a6c750 634 Phase1.write(0);
simontruelove 9:061600a6c750 635 Phase2.write(0);
simontruelove 9:061600a6c750 636 Phase3.write(0);
simontruelove 9:061600a6c750 637 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 638 }
simontruelove 3:4249dbdf7ed3 639
simontruelove 20:dca9f4c12fe3 640 void Ph41 (void) //Turn on Phase 4 and 1
simontruelove 3:4249dbdf7ed3 641 {
simontruelove 9:061600a6c750 642 Phase1.write(duty);
simontruelove 9:061600a6c750 643 Phase2.write(0);
simontruelove 9:061600a6c750 644 Phase3.write(0);
simontruelove 9:061600a6c750 645 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 646 }
simontruelove 3:4249dbdf7ed3 647
simontruelove 20:dca9f4c12fe3 648 void Initialisation (void) //Turn everything off
simontruelove 3:4249dbdf7ed3 649 {
simontruelove 9:061600a6c750 650 Phase1.write(0);
simontruelove 9:061600a6c750 651 Phase2.write(0);
simontruelove 9:061600a6c750 652 Phase3.write(0);
simontruelove 9:061600a6c750 653 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 654 led1 = 0;
simontruelove 3:4249dbdf7ed3 655 led2 = 0;
simontruelove 3:4249dbdf7ed3 656 led3 = 0;
simontruelove 3:4249dbdf7ed3 657 led4 = 0;
simontruelove 12:cbea987a3ec4 658 UnUsedPhase1=0;
simontruelove 12:cbea987a3ec4 659 UnUsedPhase2=0;
simontruelove 3:4249dbdf7ed3 660 wheel.ResetYay();
simontruelove 14:1eb49362a607 661 wheel.QEI::reset();
simontruelove 4:3aedc9246ae4 662 }
simontruelove 4:3aedc9246ae4 663
simontruelove 20:dca9f4c12fe3 664 void GetChar (void) //read keyboard strikes with terraterm
simontruelove 4:3aedc9246ae4 665 { if (pc.readable())
simontruelove 4:3aedc9246ae4 666 {
simontruelove 4:3aedc9246ae4 667 c = pc.getc();
simontruelove 20:dca9f4c12fe3 668 switch(c)
simontruelove 14:1eb49362a607 669 {
simontruelove 20:dca9f4c12fe3 670 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;
simontruelove 20:dca9f4c12fe3 671 case '1': Ph1();break; //Check Phase 1
simontruelove 20:dca9f4c12fe3 672 case '2': Ph2();break; //Check Phase 2
simontruelove 20:dca9f4c12fe3 673 case '3': Ph3();break; //Check Phase 3
simontruelove 20:dca9f4c12fe3 674 case '4': Ph4();break; //Check Phase 4
simontruelove 20:dca9f4c12fe3 675 case '5': Ph0();break; //Turn off all Phases
simontruelove 20:dca9f4c12fe3 676 case '6': StepACW();break; //Step ACW
simontruelove 20:dca9f4c12fe3 677 case '7': pc.printf("TimePerClick = %i, TimePerRev = %.5f, rps = %.5f, rpm = %.1f, \n\r", TimePerClick, TimePerRev, rps, rpm);break; //Print rpm related data
simontruelove 20:dca9f4c12fe3 678 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
simontruelove 20:dca9f4c12fe3 679 case '0': pc.printf("%i, %.5f, %i, %i, %.1f, %f, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, gain, p);break; //Print general data
simontruelove 20:dca9f4c12fe3 680 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
simontruelove 20:dca9f4c12fe3 681 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
simontruelove 20:dca9f4c12fe3 682 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
simontruelove 20:dca9f4c12fe3 683 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
simontruelove 20:dca9f4c12fe3 684 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
simontruelove 20:dca9f4c12fe3 685 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
simontruelove 20:dca9f4c12fe3 686 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
simontruelove 20:dca9f4c12fe3 687 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
simontruelove 20:dca9f4c12fe3 688 case 't': pc.printf("%.0f C\n\r",temp);break; //Print current Temp
simontruelove 20:dca9f4c12fe3 689 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+
simontruelove 20:dca9f4c12fe3 690 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-
simontruelove 20:dca9f4c12fe3 691 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+
simontruelove 20:dca9f4c12fe3 692 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-
simontruelove 20:dca9f4c12fe3 693 case 'z': led1 = !led1;led2 = 0;pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //Run motor CW
simontruelove 20:dca9f4c12fe3 694 case 'x': led1 = 0;led2 = !led2;pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //Run motor ACW
simontruelove 14:1eb49362a607 695 }
simontruelove 4:3aedc9246ae4 696 }
simontruelove 5:4e5c644d5cc3 697 }
simontruelove 5:4e5c644d5cc3 698
simontruelove 5:4e5c644d5cc3 699 void RPM (void)
simontruelove 5:4e5c644d5cc3 700 {
simontruelove 17:19b2c598810a 701 wheel.ResetWhoop(); //Whoop = 1 x per rev
simontruelove 14:1eb49362a607 702 TimePerClick = (t.read_us()); //read timer in microseconds
simontruelove 14:1eb49362a607 703 t.reset(); //reset timer
simontruelove 20:dca9f4c12fe3 704 TimePerRev = TimePerClick; //Convert from int to float TimePerClick = int, TimePerRev = float
simontruelove 14:1eb49362a607 705 TimePerRev = TimePerRev / 1000000; // 1microsecond = 0.000001s
simontruelove 20:dca9f4c12fe3 706 rps = 1 / TimePerRev; //inverse to convert SPR to rps
simontruelove 20:dca9f4c12fe3 707 rpm = rps * 60; //x 60 to convert rps to RPM
simontruelove 20:dca9f4c12fe3 708
simontruelove 21:b831f68ce5ed 709 if(duty < 0.9) //Mapped Phase Advance with no load
simontruelove 20:dca9f4c12fe3 710 {
simontruelove 21:b831f68ce5ed 711 if(rpm < 135)
simontruelove 20:dca9f4c12fe3 712 {
simontruelove 21:b831f68ce5ed 713 AdjCW = (0);
simontruelove 21:b831f68ce5ed 714 AdjACW = (13);
simontruelove 21:b831f68ce5ed 715 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 716 }
simontruelove 21:b831f68ce5ed 717 else if(rpm > 135.2 and rpm < 171)
simontruelove 20:dca9f4c12fe3 718 {
simontruelove 21:b831f68ce5ed 719 AdjCW = (1);
simontruelove 21:b831f68ce5ed 720 AdjACW = (13);
simontruelove 21:b831f68ce5ed 721 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 722 }
simontruelove 21:b831f68ce5ed 723 else if(rpm > 171 and rpm < 188)
simontruelove 20:dca9f4c12fe3 724 {
simontruelove 21:b831f68ce5ed 725 AdjCW = (1);
simontruelove 21:b831f68ce5ed 726 AdjACW = (11);
simontruelove 21:b831f68ce5ed 727 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 728 }
simontruelove 21:b831f68ce5ed 729 else if(rpm > 188 and rpm < 245)
simontruelove 20:dca9f4c12fe3 730 {
simontruelove 21:b831f68ce5ed 731 AdjCW = (2);
simontruelove 21:b831f68ce5ed 732 AdjACW = (11);
simontruelove 21:b831f68ce5ed 733 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 734 }
simontruelove 21:b831f68ce5ed 735 else if(rpm > 245 and rpm < 248)
simontruelove 20:dca9f4c12fe3 736 {
simontruelove 21:b831f68ce5ed 737 AdjCW = (2);
simontruelove 21:b831f68ce5ed 738 AdjACW = (10);
simontruelove 21:b831f68ce5ed 739 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 740 }
simontruelove 21:b831f68ce5ed 741 else if(rpm > 248 and rpm < 291)
simontruelove 20:dca9f4c12fe3 742 {
simontruelove 21:b831f68ce5ed 743 AdjCW = (3);
simontruelove 21:b831f68ce5ed 744 AdjACW = (10);
simontruelove 21:b831f68ce5ed 745 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 746 }
simontruelove 21:b831f68ce5ed 747 else if(rpm > 291 and rpm < 322)
simontruelove 20:dca9f4c12fe3 748 {
simontruelove 21:b831f68ce5ed 749 AdjCW = (3);
simontruelove 21:b831f68ce5ed 750 AdjACW = (9);
simontruelove 21:b831f68ce5ed 751 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 752 }
simontruelove 21:b831f68ce5ed 753 else if(rpm > 322 and rpm < 328)
simontruelove 20:dca9f4c12fe3 754 {
simontruelove 21:b831f68ce5ed 755 AdjCW = (3);
simontruelove 21:b831f68ce5ed 756 AdjACW = (8);
simontruelove 21:b831f68ce5ed 757 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 758 }
simontruelove 21:b831f68ce5ed 759 else if(rpm > 328 and rpm < 394)
simontruelove 20:dca9f4c12fe3 760 {
simontruelove 21:b831f68ce5ed 761 AdjCW = (4);
simontruelove 21:b831f68ce5ed 762 AdjACW = (8);
simontruelove 21:b831f68ce5ed 763 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 764 }
simontruelove 21:b831f68ce5ed 765 else if(rpm > 394 and rpm < 435)
simontruelove 20:dca9f4c12fe3 766 {
simontruelove 21:b831f68ce5ed 767 AdjCW = (4);
simontruelove 21:b831f68ce5ed 768 AdjACW = (7);
simontruelove 21:b831f68ce5ed 769 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 770 }
simontruelove 21:b831f68ce5ed 771 else if(rpm > 435 and rpm < 459)
simontruelove 20:dca9f4c12fe3 772 {
simontruelove 21:b831f68ce5ed 773 AdjCW = (5);
simontruelove 21:b831f68ce5ed 774 AdjACW = (7);
simontruelove 21:b831f68ce5ed 775 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 776 }
simontruelove 21:b831f68ce5ed 777 else if(rpm > 459 and rpm < 512 )
simontruelove 20:dca9f4c12fe3 778 {
simontruelove 21:b831f68ce5ed 779 AdjCW = (5);
simontruelove 21:b831f68ce5ed 780 AdjACW = (6);
simontruelove 21:b831f68ce5ed 781 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 782 }
simontruelove 21:b831f68ce5ed 783 else if(rpm > 512 and rpm < 533)
simontruelove 20:dca9f4c12fe3 784 {
simontruelove 21:b831f68ce5ed 785 AdjCW = (6);
simontruelove 21:b831f68ce5ed 786 AdjACW = (6);
simontruelove 21:b831f68ce5ed 787 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 788 }
simontruelove 21:b831f68ce5ed 789 else if(rpm > 533 and rpm < 641)
simontruelove 20:dca9f4c12fe3 790 {
simontruelove 21:b831f68ce5ed 791 AdjCW = (6);
simontruelove 21:b831f68ce5ed 792 AdjACW = (5);
simontruelove 21:b831f68ce5ed 793 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 794 }
simontruelove 21:b831f68ce5ed 795 else if(rpm > 641 and rpm < 763)
simontruelove 20:dca9f4c12fe3 796 {
simontruelove 21:b831f68ce5ed 797 AdjCW = (7);
simontruelove 21:b831f68ce5ed 798 AdjACW = (5);
simontruelove 21:b831f68ce5ed 799 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 800 }
simontruelove 21:b831f68ce5ed 801 else if(rpm > 763 and rpm < 1002)
simontruelove 20:dca9f4c12fe3 802 {
simontruelove 21:b831f68ce5ed 803 AdjCW = (7);
simontruelove 21:b831f68ce5ed 804 AdjACW = (4);
simontruelove 21:b831f68ce5ed 805 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 806 }
simontruelove 21:b831f68ce5ed 807 else if(rpm > 1002 and rpm < 1318)
simontruelove 20:dca9f4c12fe3 808 {
simontruelove 21:b831f68ce5ed 809 AdjCW = (8);
simontruelove 21:b831f68ce5ed 810 AdjACW = (4);
simontruelove 21:b831f68ce5ed 811 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 812 }
simontruelove 21:b831f68ce5ed 813 else if(rpm > 1318 and rpm < 1338)
simontruelove 20:dca9f4c12fe3 814 {
simontruelove 21:b831f68ce5ed 815 AdjCW = (8);
simontruelove 21:b831f68ce5ed 816 AdjACW = (3);
simontruelove 21:b831f68ce5ed 817 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 818 }
simontruelove 21:b831f68ce5ed 819 else if(rpm > 1338 and rpm < 1463)
simontruelove 20:dca9f4c12fe3 820 {
simontruelove 21:b831f68ce5ed 821 AdjCW = (9);
simontruelove 21:b831f68ce5ed 822 AdjACW = (3);
simontruelove 21:b831f68ce5ed 823 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 824 }
simontruelove 21:b831f68ce5ed 825 else if(rpm > 1463 and rpm < 1493)
simontruelove 20:dca9f4c12fe3 826 {
simontruelove 21:b831f68ce5ed 827 AdjCW = (9);
simontruelove 21:b831f68ce5ed 828 AdjACW = (2);
simontruelove 21:b831f68ce5ed 829 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 830 }
simontruelove 21:b831f68ce5ed 831 else if(rpm > 1493 and rpm < 1719)
simontruelove 20:dca9f4c12fe3 832 {
simontruelove 21:b831f68ce5ed 833 AdjCW = (10);
simontruelove 21:b831f68ce5ed 834 AdjACW = (2);
simontruelove 21:b831f68ce5ed 835 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 836 }
simontruelove 21:b831f68ce5ed 837 else if(rpm > 1719 and rpm < 1780)
simontruelove 20:dca9f4c12fe3 838 {
simontruelove 21:b831f68ce5ed 839 AdjCW = (11);
simontruelove 21:b831f68ce5ed 840 AdjACW = (2);
simontruelove 21:b831f68ce5ed 841 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 842 }
simontruelove 21:b831f68ce5ed 843 else if(rpm > 1780 and rpm < 2030)
simontruelove 20:dca9f4c12fe3 844 {
simontruelove 21:b831f68ce5ed 845 AdjCW = (11);
simontruelove 21:b831f68ce5ed 846 AdjACW = (1);
simontruelove 21:b831f68ce5ed 847 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 848 }
simontruelove 21:b831f68ce5ed 849 else if(rpm > 2030 and rpm < 2073)
simontruelove 20:dca9f4c12fe3 850 {
simontruelove 21:b831f68ce5ed 851 AdjCW = (12);
simontruelove 21:b831f68ce5ed 852 AdjACW = (1);
simontruelove 21:b831f68ce5ed 853 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 854 }
simontruelove 21:b831f68ce5ed 855 else if(rpm > 2073 and rpm < 2255)
simontruelove 20:dca9f4c12fe3 856 {
simontruelove 21:b831f68ce5ed 857 AdjCW = (12);
simontruelove 21:b831f68ce5ed 858 AdjACW = (0);
simontruelove 21:b831f68ce5ed 859 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 860 }
simontruelove 21:b831f68ce5ed 861 else if(rpm > 2255 and rpm < 2463)
simontruelove 20:dca9f4c12fe3 862 {
simontruelove 21:b831f68ce5ed 863 AdjCW = (13);
simontruelove 21:b831f68ce5ed 864 AdjACW = (0);
simontruelove 21:b831f68ce5ed 865 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 866 }
simontruelove 21:b831f68ce5ed 867 else if(rpm > 2463 and rpm < 2479)
simontruelove 20:dca9f4c12fe3 868 {
simontruelove 21:b831f68ce5ed 869 AdjCW = (14);
simontruelove 21:b831f68ce5ed 870 AdjACW = (0);
simontruelove 21:b831f68ce5ed 871 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 872 }
simontruelove 21:b831f68ce5ed 873 else if(rpm > 2479 and rpm < 2591)
simontruelove 20:dca9f4c12fe3 874 {
simontruelove 21:b831f68ce5ed 875 AdjCW = (14);
simontruelove 21:b831f68ce5ed 876 AdjACW = (63);
simontruelove 21:b831f68ce5ed 877 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 878 }
simontruelove 21:b831f68ce5ed 879 else if(rpm > 2591 and rpm < 2612)
simontruelove 20:dca9f4c12fe3 880 {
simontruelove 21:b831f68ce5ed 881 AdjCW = (14);
simontruelove 21:b831f68ce5ed 882 AdjACW = (62);
simontruelove 21:b831f68ce5ed 883 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 884 }
simontruelove 21:b831f68ce5ed 885 else if(rpm > 2612 and rpm < 2742)
simontruelove 20:dca9f4c12fe3 886 {
simontruelove 21:b831f68ce5ed 887 AdjCW = (15);
simontruelove 21:b831f68ce5ed 888 AdjACW = (62);
simontruelove 21:b831f68ce5ed 889 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 890 }
simontruelove 21:b831f68ce5ed 891 else if(rpm > 2742 and rpm < 2762)
simontruelove 20:dca9f4c12fe3 892 {
simontruelove 21:b831f68ce5ed 893 AdjCW = (16);
simontruelove 21:b831f68ce5ed 894 AdjACW = (62);
simontruelove 21:b831f68ce5ed 895 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 896 }
simontruelove 21:b831f68ce5ed 897 else if(rpm > 2762 and rpm < 2894)
simontruelove 20:dca9f4c12fe3 898 {
simontruelove 21:b831f68ce5ed 899 AdjCW = (16);
simontruelove 21:b831f68ce5ed 900 AdjACW = (61);
simontruelove 21:b831f68ce5ed 901 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 902 }
simontruelove 21:b831f68ce5ed 903 else if(rpm > 2894 and rpm < 2926)
simontruelove 20:dca9f4c12fe3 904 {
simontruelove 21:b831f68ce5ed 905 AdjCW = (17);
simontruelove 21:b831f68ce5ed 906 AdjACW = (61);
simontruelove 21:b831f68ce5ed 907 gain = (0.000001);
simontruelove 20:dca9f4c12fe3 908 }
simontruelove 21:b831f68ce5ed 909 else if(rpm > 2926 and rpm < 2982)
simontruelove 20:dca9f4c12fe3 910 {
simontruelove 21:b831f68ce5ed 911 AdjCW = (17);
simontruelove 21:b831f68ce5ed 912 AdjACW = (59);
simontruelove 21:b831f68ce5ed 913 gain = (0.000001);
simontruelove 20:dca9f4c12fe3 914 }
simontruelove 21:b831f68ce5ed 915 else if(rpm > 2982)
simontruelove 20:dca9f4c12fe3 916 {
simontruelove 21:b831f68ce5ed 917 AdjCW = (17);
simontruelove 21:b831f68ce5ed 918 AdjACW = (57);
simontruelove 21:b831f68ce5ed 919 gain = (0.000001);
simontruelove 20:dca9f4c12fe3 920 }
simontruelove 14:1eb49362a607 921 }
simontruelove 20:dca9f4c12fe3 922
simontruelove 20:dca9f4c12fe3 923 if(duty > 0.99) //Mapped Phase Advance with load
simontruelove 14:1eb49362a607 924 {
simontruelove 19:d9ba2f225f39 925 if(rpm < 48)
simontruelove 19:d9ba2f225f39 926 {
simontruelove 19:d9ba2f225f39 927 AdjCW = (52);
simontruelove 19:d9ba2f225f39 928 AdjACW = (19);
simontruelove 21:b831f68ce5ed 929 gain = (0.0001);
simontruelove 19:d9ba2f225f39 930 }
simontruelove 20:dca9f4c12fe3 931 else if(rpm > 48 and rpm < 56)
simontruelove 19:d9ba2f225f39 932 {
simontruelove 19:d9ba2f225f39 933 AdjCW = (53);
simontruelove 19:d9ba2f225f39 934 AdjACW = (19);
simontruelove 21:b831f68ce5ed 935 gain = (0.0001);
simontruelove 19:d9ba2f225f39 936 }
simontruelove 20:dca9f4c12fe3 937 else if(rpm > 56 and rpm < 76)
simontruelove 19:d9ba2f225f39 938 {
simontruelove 19:d9ba2f225f39 939 AdjCW = (55);
simontruelove 19:d9ba2f225f39 940 AdjACW = (19);
simontruelove 21:b831f68ce5ed 941 gain = (0.0001);
simontruelove 19:d9ba2f225f39 942 }
simontruelove 20:dca9f4c12fe3 943 else if(rpm > 76 and rpm < 90)
simontruelove 19:d9ba2f225f39 944 {
simontruelove 19:d9ba2f225f39 945 AdjCW = (58);
simontruelove 19:d9ba2f225f39 946 AdjACW = (19);
simontruelove 21:b831f68ce5ed 947 gain = (0.0001);
simontruelove 19:d9ba2f225f39 948 }
simontruelove 20:dca9f4c12fe3 949 else if(rpm > 90 and rpm < 125)
simontruelove 16:e6c8df9960c6 950 {
simontruelove 19:d9ba2f225f39 951 AdjCW = (58);
simontruelove 19:d9ba2f225f39 952 AdjACW = (14);
simontruelove 21:b831f68ce5ed 953 gain = (0.0001);
simontruelove 19:d9ba2f225f39 954 }
simontruelove 20:dca9f4c12fe3 955 else if(rpm > 125 and rpm < 140)
simontruelove 19:d9ba2f225f39 956 {
simontruelove 19:d9ba2f225f39 957 AdjCW = (62);
simontruelove 17:19b2c598810a 958 AdjACW = (14);
simontruelove 21:b831f68ce5ed 959 gain = (0.0001);
simontruelove 19:d9ba2f225f39 960 }
simontruelove 20:dca9f4c12fe3 961 else if(rpm > 140 and rpm < 190)
simontruelove 19:d9ba2f225f39 962 {
simontruelove 19:d9ba2f225f39 963 AdjCW = (62);
simontruelove 19:d9ba2f225f39 964 AdjACW = (9);
simontruelove 21:b831f68ce5ed 965 gain = (0.0001);
simontruelove 19:d9ba2f225f39 966 }
simontruelove 20:dca9f4c12fe3 967 else if(rpm > 190 and rpm < 200)
simontruelove 16:e6c8df9960c6 968 {
simontruelove 19:d9ba2f225f39 969 AdjCW = (3);
simontruelove 19:d9ba2f225f39 970 AdjACW = (9);
simontruelove 21:b831f68ce5ed 971 gain = (0.0001);
simontruelove 19:d9ba2f225f39 972 }
simontruelove 20:dca9f4c12fe3 973 else if(rpm > 200 and rpm < 350)
simontruelove 19:d9ba2f225f39 974 {
simontruelove 19:d9ba2f225f39 975 AdjCW = (3);
simontruelove 19:d9ba2f225f39 976 AdjACW = (9);
simontruelove 21:b831f68ce5ed 977 gain = (0.0001);
simontruelove 19:d9ba2f225f39 978 }
simontruelove 20:dca9f4c12fe3 979 else if(rpm > 350 and rpm < 650)
simontruelove 19:d9ba2f225f39 980 {
simontruelove 19:d9ba2f225f39 981 AdjCW = (3);
simontruelove 19:d9ba2f225f39 982 AdjACW = (5);
simontruelove 21:b831f68ce5ed 983 gain = (0.0001);
simontruelove 19:d9ba2f225f39 984 }
simontruelove 20:dca9f4c12fe3 985 else if(rpm > 650)
simontruelove 19:d9ba2f225f39 986 {
simontruelove 19:d9ba2f225f39 987 AdjCW = (6);
simontruelove 16:e6c8df9960c6 988 AdjACW = (5);
simontruelove 21:b831f68ce5ed 989 gain = (0.000001);
simontruelove 16:e6c8df9960c6 990 }
simontruelove 14:1eb49362a607 991 }
simontruelove 7:b8de1529c7fc 992 }
simontruelove 7:b8de1529c7fc 993
simontruelove 7:b8de1529c7fc 994 void VelocityLoop (void)
simontruelove 7:b8de1529c7fc 995 {
simontruelove 20:dca9f4c12fe3 996 diff = SetPoint - rpm; //difference between setpoint and the RPM measurement
simontruelove 20:dca9f4c12fe3 997 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
simontruelove 20:dca9f4c12fe3 998
simontruelove 20:dca9f4c12fe3 999 if (duty > 1) //limits for duty. Motor will not operate below 0.11. 1 = max
simontruelove 14:1eb49362a607 1000 {
simontruelove 9:061600a6c750 1001 duty = 1;
simontruelove 7:b8de1529c7fc 1002 }
simontruelove 20:dca9f4c12fe3 1003 if (duty <0.11) //min duty 0.11
simontruelove 10:808cb9052f14 1004 {
simontruelove 19:d9ba2f225f39 1005 duty = 0.11;
simontruelove 7:b8de1529c7fc 1006 }
simontruelove 11:74eeb8871fe6 1007 }
simontruelove 11:74eeb8871fe6 1008
simontruelove 20:dca9f4c12fe3 1009 void ReadKType(void) //Reads Temperature
simontruelove 11:74eeb8871fe6 1010 {
simontruelove 11:74eeb8871fe6 1011 int i = 0;
simontruelove 11:74eeb8871fe6 1012 int Readout = 0;
simontruelove 11:74eeb8871fe6 1013
simontruelove 11:74eeb8871fe6 1014 cs1 = 0;
simontruelove 20:dca9f4c12fe3 1015 SerialClock = 0; //set clock to 0
simontruelove 16:e6c8df9960c6 1016 wait_ms(0.01);
simontruelove 20:dca9f4c12fe3 1017 SerialClock = 1; //clock once to set to the 13 bit temp data
simontruelove 16:e6c8df9960c6 1018 wait_ms(0.01);
simontruelove 11:74eeb8871fe6 1019 SerialClock = 0;
simontruelove 16:e6c8df9960c6 1020 wait_ms(0.01);
simontruelove 11:74eeb8871fe6 1021
simontruelove 20:dca9f4c12fe3 1022 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
simontruelove 11:74eeb8871fe6 1023 {
simontruelove 20:dca9f4c12fe3 1024 if(DOut == 1) //check data, store results in readout
simontruelove 11:74eeb8871fe6 1025 {
simontruelove 12:cbea987a3ec4 1026 Readout |= (1<<i);
simontruelove 11:74eeb8871fe6 1027 }
simontruelove 11:74eeb8871fe6 1028 else
simontruelove 11:74eeb8871fe6 1029 {
simontruelove 11:74eeb8871fe6 1030 Readout |= (0<<i);
simontruelove 11:74eeb8871fe6 1031 }
simontruelove 12:cbea987a3ec4 1032
simontruelove 20:dca9f4c12fe3 1033 SerialClock = 1; //clock to the next bit
simontruelove 16:e6c8df9960c6 1034 wait_ms(0.01);
simontruelove 11:74eeb8871fe6 1035 SerialClock = 0;
simontruelove 16:e6c8df9960c6 1036 wait_ms(0.01);
simontruelove 11:74eeb8871fe6 1037 }
simontruelove 20:dca9f4c12fe3 1038 temp = Readout * 0.125; //get the real temp value which is a float
simontruelove 11:74eeb8871fe6 1039 Readout = 0;
simontruelove 11:74eeb8871fe6 1040 cs1 = 1;
simontruelove 3:4249dbdf7ed3 1041 }