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@20:dca9f4c12fe3, 2019-04-11 (annotated)

Committer:
simontruelove
Date:
Thu Apr 11 14:53:32 2019 +0000
Revision:
20:dca9f4c12fe3
Parent:
19:d9ba2f225f39
Child:
21:b831f68ce5ed
Tidy code

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 20:dca9f4c12fe3 58 int SetPoint = 1000; //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 20:dca9f4c12fe3 71 float gain = 0.001; //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 20:dca9f4c12fe3 709 if(duty < 1) //Mapped Phase Advance with no load
simontruelove 20:dca9f4c12fe3 710 {
simontruelove 20:dca9f4c12fe3 711 if(rpm < 135.2 )
simontruelove 20:dca9f4c12fe3 712 {
simontruelove 20:dca9f4c12fe3 713 AdjCW = ( 0 );
simontruelove 20:dca9f4c12fe3 714 AdjACW = ( 13 );
simontruelove 20:dca9f4c12fe3 715 }
simontruelove 20:dca9f4c12fe3 716 else if(rpm > 135.2 and rpm < 171.3 )
simontruelove 20:dca9f4c12fe3 717 {
simontruelove 20:dca9f4c12fe3 718 AdjCW = ( 1 );
simontruelove 20:dca9f4c12fe3 719 AdjACW = ( 13 );
simontruelove 20:dca9f4c12fe3 720 }
simontruelove 20:dca9f4c12fe3 721 else if(rpm > 171.3 and rpm < 188.7 )
simontruelove 20:dca9f4c12fe3 722 {
simontruelove 20:dca9f4c12fe3 723 AdjCW = ( 1 );
simontruelove 20:dca9f4c12fe3 724 AdjACW = ( 11 );
simontruelove 20:dca9f4c12fe3 725 }
simontruelove 20:dca9f4c12fe3 726 else if(rpm > 188.7 and rpm < 245.3 )
simontruelove 20:dca9f4c12fe3 727 {
simontruelove 20:dca9f4c12fe3 728 AdjCW = ( 2 );
simontruelove 20:dca9f4c12fe3 729 AdjACW = ( 11 );
simontruelove 20:dca9f4c12fe3 730 }
simontruelove 20:dca9f4c12fe3 731 else if(rpm > 245.3 and rpm < 248.9 )
simontruelove 20:dca9f4c12fe3 732 {
simontruelove 20:dca9f4c12fe3 733 AdjCW = ( 2 );
simontruelove 20:dca9f4c12fe3 734 AdjACW = ( 10 );
simontruelove 20:dca9f4c12fe3 735 }
simontruelove 20:dca9f4c12fe3 736 else if(rpm > 248.9 and rpm < 291.3 )
simontruelove 20:dca9f4c12fe3 737 {
simontruelove 20:dca9f4c12fe3 738 AdjCW = ( 3 );
simontruelove 20:dca9f4c12fe3 739 AdjACW = ( 10 );
simontruelove 20:dca9f4c12fe3 740 }
simontruelove 20:dca9f4c12fe3 741 else if(rpm > 291.3 and rpm < 322.9 )
simontruelove 20:dca9f4c12fe3 742 {
simontruelove 20:dca9f4c12fe3 743 AdjCW = ( 3 );
simontruelove 20:dca9f4c12fe3 744 AdjACW = ( 9 );
simontruelove 20:dca9f4c12fe3 745 }
simontruelove 20:dca9f4c12fe3 746 else if(rpm > 322.9 and rpm < 328.5 )
simontruelove 20:dca9f4c12fe3 747 {
simontruelove 20:dca9f4c12fe3 748 AdjCW = ( 3 );
simontruelove 20:dca9f4c12fe3 749 AdjACW = ( 8 );
simontruelove 20:dca9f4c12fe3 750 }
simontruelove 20:dca9f4c12fe3 751 else if(rpm > 328.5 and rpm < 394.7 )
simontruelove 20:dca9f4c12fe3 752 {
simontruelove 20:dca9f4c12fe3 753 AdjCW = ( 4 );
simontruelove 20:dca9f4c12fe3 754 AdjACW = ( 8 );
simontruelove 20:dca9f4c12fe3 755 }
simontruelove 20:dca9f4c12fe3 756 else if(rpm > 394.7 and rpm < 435.8 )
simontruelove 20:dca9f4c12fe3 757 {
simontruelove 20:dca9f4c12fe3 758 AdjCW = ( 4 );
simontruelove 20:dca9f4c12fe3 759 AdjACW = ( 7 );
simontruelove 20:dca9f4c12fe3 760 }
simontruelove 20:dca9f4c12fe3 761 else if(rpm > 435.8 and rpm < 459.3 )
simontruelove 20:dca9f4c12fe3 762 {
simontruelove 20:dca9f4c12fe3 763 AdjCW = ( 5 );
simontruelove 20:dca9f4c12fe3 764 AdjACW = ( 7 );
simontruelove 20:dca9f4c12fe3 765 }
simontruelove 20:dca9f4c12fe3 766 else if(rpm > 459.3 and rpm < 512.9 )
simontruelove 20:dca9f4c12fe3 767 {
simontruelove 20:dca9f4c12fe3 768 AdjCW = ( 5 );
simontruelove 20:dca9f4c12fe3 769 AdjACW = ( 6 );
simontruelove 20:dca9f4c12fe3 770 }
simontruelove 20:dca9f4c12fe3 771 else if(rpm > 512.9 and rpm < 533.8 )
simontruelove 20:dca9f4c12fe3 772 {
simontruelove 20:dca9f4c12fe3 773 AdjCW = ( 6 );
simontruelove 20:dca9f4c12fe3 774 AdjACW = ( 6 );
simontruelove 20:dca9f4c12fe3 775 }
simontruelove 20:dca9f4c12fe3 776 else if(rpm > 533.8 and rpm < 641.2 )
simontruelove 20:dca9f4c12fe3 777 {
simontruelove 20:dca9f4c12fe3 778 AdjCW = ( 6 );
simontruelove 20:dca9f4c12fe3 779 AdjACW = ( 5 );
simontruelove 20:dca9f4c12fe3 780 }
simontruelove 20:dca9f4c12fe3 781 else if(rpm > 641.2 and rpm < 763.1 )
simontruelove 20:dca9f4c12fe3 782 {
simontruelove 20:dca9f4c12fe3 783 AdjCW = ( 7 );
simontruelove 20:dca9f4c12fe3 784 AdjACW = ( 5 );
simontruelove 20:dca9f4c12fe3 785 }
simontruelove 20:dca9f4c12fe3 786 else if(rpm > 763.1 and rpm < 1002.3 )
simontruelove 20:dca9f4c12fe3 787 {
simontruelove 20:dca9f4c12fe3 788 AdjCW = ( 7 );
simontruelove 20:dca9f4c12fe3 789 AdjACW = ( 4 );
simontruelove 20:dca9f4c12fe3 790 }
simontruelove 20:dca9f4c12fe3 791 else if(rpm > 1002.3 and rpm < 1318.6 )
simontruelove 20:dca9f4c12fe3 792 {
simontruelove 20:dca9f4c12fe3 793 AdjCW = ( 8 );
simontruelove 20:dca9f4c12fe3 794 AdjACW = ( 4 );
simontruelove 20:dca9f4c12fe3 795 }
simontruelove 20:dca9f4c12fe3 796 else if(rpm > 1318.6 and rpm < 1338.1 )
simontruelove 20:dca9f4c12fe3 797 {
simontruelove 20:dca9f4c12fe3 798 AdjCW = ( 8 );
simontruelove 20:dca9f4c12fe3 799 AdjACW = ( 3 );
simontruelove 20:dca9f4c12fe3 800 }
simontruelove 20:dca9f4c12fe3 801 else if(rpm > 1338.1 and rpm < 1463.6 )
simontruelove 20:dca9f4c12fe3 802 {
simontruelove 20:dca9f4c12fe3 803 AdjCW = ( 9 );
simontruelove 20:dca9f4c12fe3 804 AdjACW = ( 3 );
simontruelove 20:dca9f4c12fe3 805 }
simontruelove 20:dca9f4c12fe3 806 else if(rpm > 1463.6 and rpm < 1493.7 )
simontruelove 20:dca9f4c12fe3 807 {
simontruelove 20:dca9f4c12fe3 808 AdjCW = ( 9 );
simontruelove 20:dca9f4c12fe3 809 AdjACW = ( 2 );
simontruelove 20:dca9f4c12fe3 810 }
simontruelove 20:dca9f4c12fe3 811 else if(rpm > 1493.7 and rpm < 1719.6 )
simontruelove 20:dca9f4c12fe3 812 {
simontruelove 20:dca9f4c12fe3 813 AdjCW = ( 10 );
simontruelove 20:dca9f4c12fe3 814 AdjACW = ( 2 );
simontruelove 20:dca9f4c12fe3 815 }
simontruelove 20:dca9f4c12fe3 816 else if(rpm > 1719.6 and rpm < 1780.5 )
simontruelove 20:dca9f4c12fe3 817 {
simontruelove 20:dca9f4c12fe3 818 AdjCW = ( 11 );
simontruelove 20:dca9f4c12fe3 819 AdjACW = ( 2 );
simontruelove 20:dca9f4c12fe3 820 }
simontruelove 20:dca9f4c12fe3 821 else if(rpm > 1780.5 and rpm < 2030.5 )
simontruelove 20:dca9f4c12fe3 822 {
simontruelove 20:dca9f4c12fe3 823 AdjCW = ( 11 );
simontruelove 20:dca9f4c12fe3 824 AdjACW = ( 1 );
simontruelove 20:dca9f4c12fe3 825 }
simontruelove 20:dca9f4c12fe3 826 else if(rpm > 2030.5 and rpm < 2073.1 )
simontruelove 20:dca9f4c12fe3 827 {
simontruelove 20:dca9f4c12fe3 828 AdjCW = ( 12 );
simontruelove 20:dca9f4c12fe3 829 AdjACW = ( 1 );
simontruelove 20:dca9f4c12fe3 830 }
simontruelove 20:dca9f4c12fe3 831 else if(rpm > 2073.1 and rpm < 2255.2 )
simontruelove 20:dca9f4c12fe3 832 {
simontruelove 20:dca9f4c12fe3 833 AdjCW = ( 12 );
simontruelove 20:dca9f4c12fe3 834 AdjACW = ( 0 );
simontruelove 20:dca9f4c12fe3 835 }
simontruelove 20:dca9f4c12fe3 836 else if(rpm > 2255.2 and rpm < 2463.3 )
simontruelove 20:dca9f4c12fe3 837 {
simontruelove 20:dca9f4c12fe3 838 AdjCW = ( 13 );
simontruelove 20:dca9f4c12fe3 839 AdjACW = ( 0 );
simontruelove 20:dca9f4c12fe3 840 }
simontruelove 20:dca9f4c12fe3 841 else if(rpm > 2463.3 and rpm < 2479.4 )
simontruelove 20:dca9f4c12fe3 842 {
simontruelove 20:dca9f4c12fe3 843 AdjCW = ( 14 );
simontruelove 20:dca9f4c12fe3 844 AdjACW = ( 0 );
simontruelove 20:dca9f4c12fe3 845 }
simontruelove 20:dca9f4c12fe3 846 else if(rpm > 2479.4 and rpm < 2591 )
simontruelove 20:dca9f4c12fe3 847 {
simontruelove 20:dca9f4c12fe3 848 AdjCW = ( 14 );
simontruelove 20:dca9f4c12fe3 849 AdjACW = ( 63 );
simontruelove 20:dca9f4c12fe3 850 }
simontruelove 20:dca9f4c12fe3 851 else if(rpm > 2591 and rpm < 2612.9 )
simontruelove 20:dca9f4c12fe3 852 {
simontruelove 20:dca9f4c12fe3 853 AdjCW = ( 14 );
simontruelove 20:dca9f4c12fe3 854 AdjACW = ( 62 );
simontruelove 20:dca9f4c12fe3 855 }
simontruelove 20:dca9f4c12fe3 856 else if(rpm > 2612.9 and rpm < 2742.4 )
simontruelove 20:dca9f4c12fe3 857 {
simontruelove 20:dca9f4c12fe3 858 AdjCW = ( 15 );
simontruelove 20:dca9f4c12fe3 859 AdjACW = ( 62 );
simontruelove 20:dca9f4c12fe3 860 }
simontruelove 20:dca9f4c12fe3 861 else if(rpm > 2742.4 and rpm < 2762.3 )
simontruelove 20:dca9f4c12fe3 862 {
simontruelove 20:dca9f4c12fe3 863 AdjCW = ( 16 );
simontruelove 20:dca9f4c12fe3 864 AdjACW = ( 62 );
simontruelove 20:dca9f4c12fe3 865 }
simontruelove 20:dca9f4c12fe3 866 else if(rpm > 2762.3 and rpm < 2894.6 )
simontruelove 20:dca9f4c12fe3 867 {
simontruelove 20:dca9f4c12fe3 868 AdjCW = ( 16 );
simontruelove 20:dca9f4c12fe3 869 AdjACW = ( 61 );
simontruelove 20:dca9f4c12fe3 870 }
simontruelove 20:dca9f4c12fe3 871 else if(rpm > 2894.6 and rpm < 2926.7 )
simontruelove 20:dca9f4c12fe3 872 {
simontruelove 20:dca9f4c12fe3 873 AdjCW = ( 17 );
simontruelove 20:dca9f4c12fe3 874 AdjACW = ( 61 );
simontruelove 20:dca9f4c12fe3 875 }
simontruelove 20:dca9f4c12fe3 876 else if(rpm > 2926.7 and rpm < 2982 )
simontruelove 20:dca9f4c12fe3 877 {
simontruelove 20:dca9f4c12fe3 878 AdjCW = ( 17 );
simontruelove 20:dca9f4c12fe3 879 AdjACW = ( 59 );
simontruelove 20:dca9f4c12fe3 880 }
simontruelove 20:dca9f4c12fe3 881 else if(rpm > 2982 )
simontruelove 20:dca9f4c12fe3 882 {
simontruelove 20:dca9f4c12fe3 883 AdjCW = ( 17 );
simontruelove 20:dca9f4c12fe3 884 AdjACW = ( 57 );
simontruelove 20:dca9f4c12fe3 885 }
simontruelove 14:1eb49362a607 886 }
simontruelove 20:dca9f4c12fe3 887
simontruelove 20:dca9f4c12fe3 888 if(duty > 0.99) //Mapped Phase Advance with load
simontruelove 14:1eb49362a607 889 {
simontruelove 19:d9ba2f225f39 890 if(rpm < 48)
simontruelove 19:d9ba2f225f39 891 {
simontruelove 19:d9ba2f225f39 892 AdjCW = (52);
simontruelove 19:d9ba2f225f39 893 AdjACW = (19);
simontruelove 19:d9ba2f225f39 894 }
simontruelove 20:dca9f4c12fe3 895 else if(rpm > 48 and rpm < 56)
simontruelove 19:d9ba2f225f39 896 {
simontruelove 19:d9ba2f225f39 897 AdjCW = (53);
simontruelove 19:d9ba2f225f39 898 AdjACW = (19);
simontruelove 19:d9ba2f225f39 899 }
simontruelove 20:dca9f4c12fe3 900 else if(rpm > 56 and rpm < 76)
simontruelove 19:d9ba2f225f39 901 {
simontruelove 19:d9ba2f225f39 902 AdjCW = (55);
simontruelove 19:d9ba2f225f39 903 AdjACW = (19);
simontruelove 19:d9ba2f225f39 904 }
simontruelove 20:dca9f4c12fe3 905 else if(rpm > 76 and rpm < 90)
simontruelove 19:d9ba2f225f39 906 {
simontruelove 19:d9ba2f225f39 907 AdjCW = (58);
simontruelove 19:d9ba2f225f39 908 AdjACW = (19);
simontruelove 19:d9ba2f225f39 909 }
simontruelove 20:dca9f4c12fe3 910 else if(rpm > 90 and rpm < 125)
simontruelove 16:e6c8df9960c6 911 {
simontruelove 19:d9ba2f225f39 912 AdjCW = (58);
simontruelove 19:d9ba2f225f39 913 AdjACW = (14);
simontruelove 19:d9ba2f225f39 914 }
simontruelove 20:dca9f4c12fe3 915 else if(rpm > 125 and rpm < 140)
simontruelove 19:d9ba2f225f39 916 {
simontruelove 19:d9ba2f225f39 917 AdjCW = (62);
simontruelove 17:19b2c598810a 918 AdjACW = (14);
simontruelove 19:d9ba2f225f39 919 }
simontruelove 20:dca9f4c12fe3 920 else if(rpm > 140 and rpm < 190)
simontruelove 19:d9ba2f225f39 921 {
simontruelove 19:d9ba2f225f39 922 AdjCW = (62);
simontruelove 19:d9ba2f225f39 923 AdjACW = (9);
simontruelove 19:d9ba2f225f39 924 }
simontruelove 20:dca9f4c12fe3 925 else if(rpm > 190 and rpm < 200)
simontruelove 16:e6c8df9960c6 926 {
simontruelove 19:d9ba2f225f39 927 AdjCW = (3);
simontruelove 19:d9ba2f225f39 928 AdjACW = (9);
simontruelove 19:d9ba2f225f39 929 }
simontruelove 20:dca9f4c12fe3 930 else if(rpm > 200 and rpm < 350)
simontruelove 19:d9ba2f225f39 931 {
simontruelove 19:d9ba2f225f39 932 AdjCW = (3);
simontruelove 19:d9ba2f225f39 933 AdjACW = (9);
simontruelove 19:d9ba2f225f39 934 }
simontruelove 20:dca9f4c12fe3 935 else if(rpm > 350 and rpm < 650)
simontruelove 19:d9ba2f225f39 936 {
simontruelove 19:d9ba2f225f39 937 AdjCW = (3);
simontruelove 19:d9ba2f225f39 938 AdjACW = (5);
simontruelove 19:d9ba2f225f39 939 }
simontruelove 20:dca9f4c12fe3 940 else if(rpm > 650)
simontruelove 19:d9ba2f225f39 941 {
simontruelove 19:d9ba2f225f39 942 AdjCW = (6);
simontruelove 16:e6c8df9960c6 943 AdjACW = (5);
simontruelove 16:e6c8df9960c6 944 }
simontruelove 14:1eb49362a607 945 }
simontruelove 7:b8de1529c7fc 946 }
simontruelove 7:b8de1529c7fc 947
simontruelove 7:b8de1529c7fc 948 void VelocityLoop (void)
simontruelove 7:b8de1529c7fc 949 {
simontruelove 20:dca9f4c12fe3 950 diff = SetPoint - rpm; //difference between setpoint and the RPM measurement
simontruelove 20:dca9f4c12fe3 951 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 952
simontruelove 20:dca9f4c12fe3 953 if (duty > 1) //limits for duty. Motor will not operate below 0.11. 1 = max
simontruelove 14:1eb49362a607 954 {
simontruelove 9:061600a6c750 955 duty = 1;
simontruelove 7:b8de1529c7fc 956 }
simontruelove 20:dca9f4c12fe3 957 if (duty <0.11) //min duty 0.11
simontruelove 10:808cb9052f14 958 {
simontruelove 19:d9ba2f225f39 959 duty = 0.11;
simontruelove 7:b8de1529c7fc 960 }
simontruelove 11:74eeb8871fe6 961 }
simontruelove 11:74eeb8871fe6 962
simontruelove 20:dca9f4c12fe3 963 void ReadKType(void) //Reads Temperature
simontruelove 11:74eeb8871fe6 964 {
simontruelove 11:74eeb8871fe6 965 int i = 0;
simontruelove 11:74eeb8871fe6 966 int Readout = 0;
simontruelove 11:74eeb8871fe6 967
simontruelove 11:74eeb8871fe6 968 cs1 = 0;
simontruelove 20:dca9f4c12fe3 969 SerialClock = 0; //set clock to 0
simontruelove 16:e6c8df9960c6 970 wait_ms(0.01);
simontruelove 20:dca9f4c12fe3 971 SerialClock = 1; //clock once to set to the 13 bit temp data
simontruelove 16:e6c8df9960c6 972 wait_ms(0.01);
simontruelove 11:74eeb8871fe6 973 SerialClock = 0;
simontruelove 16:e6c8df9960c6 974 wait_ms(0.01);
simontruelove 11:74eeb8871fe6 975
simontruelove 20:dca9f4c12fe3 976 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 977 {
simontruelove 20:dca9f4c12fe3 978 if(DOut == 1) //check data, store results in readout
simontruelove 11:74eeb8871fe6 979 {
simontruelove 12:cbea987a3ec4 980 Readout |= (1<<i);
simontruelove 11:74eeb8871fe6 981 }
simontruelove 11:74eeb8871fe6 982 else
simontruelove 11:74eeb8871fe6 983 {
simontruelove 11:74eeb8871fe6 984 Readout |= (0<<i);
simontruelove 11:74eeb8871fe6 985 }
simontruelove 12:cbea987a3ec4 986
simontruelove 20:dca9f4c12fe3 987 SerialClock = 1; //clock to the next bit
simontruelove 16:e6c8df9960c6 988 wait_ms(0.01);
simontruelove 11:74eeb8871fe6 989 SerialClock = 0;
simontruelove 16:e6c8df9960c6 990 wait_ms(0.01);
simontruelove 11:74eeb8871fe6 991 }
simontruelove 20:dca9f4c12fe3 992 temp = Readout * 0.125; //get the real temp value which is a float
simontruelove 11:74eeb8871fe6 993 Readout = 0;
simontruelove 11:74eeb8871fe6 994 cs1 = 1;
simontruelove 3:4249dbdf7ed3 995 }