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@17:19b2c598810a, 2019-04-03 (annotated)

Committer:
simontruelove
Date:
Wed Apr 03 10:46:47 2019 +0000
Revision:
17:19b2c598810a
Parent:
16:e6c8df9960c6
Child:
18:3493de6fe8ce
April 2019 version;

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