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@19:d9ba2f225f39, 2019-04-05 (annotated)

Committer:
simontruelove
Date:
Fri Apr 05 10:31:29 2019 +0000
Revision:
19:d9ba2f225f39
Parent:
18:3493de6fe8ce
Child:
20:dca9f4c12fe3
Before code review

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