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@7:b8de1529c7fc, 2018-11-19 (annotated)

Committer:
simontruelove
Date:
Mon Nov 19 11:14:28 2018 +0000
Revision:
7:b8de1529c7fc
Parent:
6:f7028034aabb
Child:
8:2e690f407ec6
Now with stable RPM measurement

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 3:4249dbdf7ed3 4 void Initialisation (void); //These voids are written after the main. They must be listed here too (functional prototypes).
simontruelove 1:0191658b6ff4 5 void StepCW(void);
simontruelove 1:0191658b6ff4 6 void Ph1(void);
simontruelove 3:4249dbdf7ed3 7 void Ph12 (void);
simontruelove 1:0191658b6ff4 8 void Ph2(void);
simontruelove 3:4249dbdf7ed3 9 void Ph23 (void);
simontruelove 1:0191658b6ff4 10 void Ph3(void);
simontruelove 3:4249dbdf7ed3 11 void Ph34 (void);
simontruelove 1:0191658b6ff4 12 void Ph4(void);
simontruelove 3:4249dbdf7ed3 13 void Ph41 (void);
simontruelove 4:3aedc9246ae4 14 void GetChar (void);
simontruelove 5:4e5c644d5cc3 15 void RPM (void);
simontruelove 7:b8de1529c7fc 16 void VelocityLoop (void);
simontruelove 1:0191658b6ff4 17
simontruelove 1:0191658b6ff4 18 Serial pc(USBTX, USBRX); // tx, rx - set up the Terraterm input from mbed
simontruelove 1:0191658b6ff4 19
simontruelove 3:4249dbdf7ed3 20 QEI wheel(p5, p6, p8, 800, QEI::X4_ENCODING); //code for quadrature encoder see QEI.h
simontruelove 3:4249dbdf7ed3 21
simontruelove 3:4249dbdf7ed3 22 Timer t;
simontruelove 3:4249dbdf7ed3 23
simontruelove 7:b8de1529c7fc 24 PwmOut Phase1 (p21); //Pin and LED set up
simontruelove 7:b8de1529c7fc 25 PwmOut Phase2 (p22);
simontruelove 7:b8de1529c7fc 26 PwmOut Phase3 (p23);
simontruelove 7:b8de1529c7fc 27 PwmOut Phase4 (p24);
simontruelove 1:0191658b6ff4 28
simontruelove 6:f7028034aabb 29 AnalogOut Aout(p18);
simontruelove 6:f7028034aabb 30
simontruelove 2:3f95c82c26bb 31 DigitalIn Button1 (p11);
simontruelove 4:3aedc9246ae4 32 DigitalIn Button2 (p12);
simontruelove 2:3f95c82c26bb 33
simontruelove 2:3f95c82c26bb 34 DigitalOut led1(LED1);
simontruelove 2:3f95c82c26bb 35 DigitalOut led2(LED2);
simontruelove 2:3f95c82c26bb 36 DigitalOut led3(LED3);
simontruelove 2:3f95c82c26bb 37 DigitalOut led4(LED4);
simontruelove 1:0191658b6ff4 38
simontruelove 3:4249dbdf7ed3 39 int StateA = 0;
simontruelove 3:4249dbdf7ed3 40 int StateB = 0;
simontruelove 6:f7028034aabb 41 //int StateC = 0;
simontruelove 6:f7028034aabb 42 int AdjCW = 2;
simontruelove 6:f7028034aabb 43 int AdjACW = 5;
simontruelove 3:4249dbdf7ed3 44 int TimePerClick = 0;
simontruelove 3:4249dbdf7ed3 45 int TimePerRev = 0;
simontruelove 3:4249dbdf7ed3 46 int RPS = 0;
simontruelove 5:4e5c644d5cc3 47 int rpm = 0;
simontruelove 7:b8de1529c7fc 48 int SetPoint = 250;
simontruelove 7:b8de1529c7fc 49 int z = 80;
simontruelove 1:0191658b6ff4 50
simontruelove 4:3aedc9246ae4 51 char c;
simontruelove 4:3aedc9246ae4 52
simontruelove 7:b8de1529c7fc 53 float duty = 1;
simontruelove 7:b8de1529c7fc 54 float diff = 0.0;
simontruelove 7:b8de1529c7fc 55 float x=0.1; //x=time of square wave when 1 phase energised,
simontruelove 7:b8de1529c7fc 56 float y=0.04; //y=time of square wave when 2 phases energised
simontruelove 1:0191658b6ff4 57
simontruelove 1:0191658b6ff4 58 int main(void)
simontruelove 3:4249dbdf7ed3 59 {
simontruelove 1:0191658b6ff4 60 pc.baud(230400); //Set fastest baud rate
simontruelove 7:b8de1529c7fc 61 Phase1.period(0.000001);
simontruelove 7:b8de1529c7fc 62 Phase2.period(0.000001);
simontruelove 7:b8de1529c7fc 63 Phase3.period(0.000001);
simontruelove 7:b8de1529c7fc 64 Phase4.period(0.000001);
simontruelove 3:4249dbdf7ed3 65 StepCW();
simontruelove 3:4249dbdf7ed3 66 Initialisation();
simontruelove 3:4249dbdf7ed3 67 wait(0.1);
simontruelove 3:4249dbdf7ed3 68 t.start();
simontruelove 6:f7028034aabb 69
simontruelove 7:b8de1529c7fc 70
simontruelove 7:b8de1529c7fc 71 while(wheel.getRevolutions()<2)
simontruelove 6:f7028034aabb 72 {
simontruelove 5:4e5c644d5cc3 73 switch(StateA)
simontruelove 3:4249dbdf7ed3 74 {
simontruelove 6:f7028034aabb 75 case 0:Ph1();break;
simontruelove 6:f7028034aabb 76 case 1:Ph1();break;
simontruelove 6:f7028034aabb 77 case 2:Ph12();break;
simontruelove 6:f7028034aabb 78 case 3:Ph12();break;
simontruelove 6:f7028034aabb 79 case 4:Ph2();break;
simontruelove 6:f7028034aabb 80 case 5:Ph2();break;
simontruelove 6:f7028034aabb 81 case 6:Ph23();break;
simontruelove 6:f7028034aabb 82 case 7:Ph23();break;
simontruelove 6:f7028034aabb 83 case 8:Ph3();break;
simontruelove 6:f7028034aabb 84 case 9:Ph3();break;
simontruelove 6:f7028034aabb 85 case 10:Ph34();break;
simontruelove 6:f7028034aabb 86 case 11:Ph34();break;
simontruelove 6:f7028034aabb 87 case 12:Ph4();break;
simontruelove 6:f7028034aabb 88 case 13:Ph4();break;
simontruelove 6:f7028034aabb 89 case 14:Ph41();break;
simontruelove 6:f7028034aabb 90 case 15:Ph41();break;
simontruelove 5:4e5c644d5cc3 91 default:break;
simontruelove 3:4249dbdf7ed3 92 }
simontruelove 3:4249dbdf7ed3 93
simontruelove 3:4249dbdf7ed3 94 if(wheel.getYay()==1)
simontruelove 3:4249dbdf7ed3 95 {
simontruelove 5:4e5c644d5cc3 96 StateA++;
simontruelove 5:4e5c644d5cc3 97 wheel.ResetYay();
simontruelove 6:f7028034aabb 98 if (StateA>15)
simontruelove 3:4249dbdf7ed3 99 {
simontruelove 5:4e5c644d5cc3 100 StateA=0;
simontruelove 3:4249dbdf7ed3 101 }
simontruelove 3:4249dbdf7ed3 102 }
simontruelove 5:4e5c644d5cc3 103 }
simontruelove 3:4249dbdf7ed3 104
simontruelove 5:4e5c644d5cc3 105 while(1)
simontruelove 5:4e5c644d5cc3 106 {
simontruelove 4:3aedc9246ae4 107 while((led1 == 0) && (led2 == 0))
simontruelove 4:3aedc9246ae4 108 {
simontruelove 7:b8de1529c7fc 109 Aout = 0;
simontruelove 7:b8de1529c7fc 110 Phase1.write(0);
simontruelove 7:b8de1529c7fc 111 Phase2.write(0);
simontruelove 7:b8de1529c7fc 112 Phase3.write(0);
simontruelove 7:b8de1529c7fc 113 Phase4.write(0);
simontruelove 7:b8de1529c7fc 114 GetChar();
simontruelove 6:f7028034aabb 115 //StateB = wheel.getPulses()%16;
simontruelove 6:f7028034aabb 116 //StateC = (800+wheel.getPulses()+StateA+AdjCW)%16;
simontruelove 6:f7028034aabb 117 //pc.printf("StateA= %i, StateB= %i, StateC= %i, Pulses = %i\n\r", StateA, StateB, StateC, wheel.getPulses());
simontruelove 5:4e5c644d5cc3 118 //pc.printf("0 StateB= %i, Pulses= %i, Revs= %i\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 4:3aedc9246ae4 119 }
simontruelove 7:b8de1529c7fc 120 while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led1==1))
simontruelove 3:4249dbdf7ed3 121 {
simontruelove 4:3aedc9246ae4 122 GetChar();
simontruelove 6:f7028034aabb 123 StateB = (wheel.getPulses()+StateA+AdjCW)%16;
simontruelove 7:b8de1529c7fc 124 //pc.printf("rpm = %i, Whoop = %i\n\r", rpm, wheel.getWhoop());
simontruelove 6:f7028034aabb 125 //pc.printf("StateB= %i\n\r", StateB);
simontruelove 4:3aedc9246ae4 126 //pc.printf("1 StateB= %i, Pulses= %i, Revs= %i\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 4:3aedc9246ae4 127
simontruelove 4:3aedc9246ae4 128 switch(StateB)
simontruelove 4:3aedc9246ae4 129 {
simontruelove 6:f7028034aabb 130 case 0:Ph1();break;
simontruelove 6:f7028034aabb 131 case 1:Ph1();break;
simontruelove 6:f7028034aabb 132 case 2:Ph12();break;
simontruelove 6:f7028034aabb 133 case 3:Ph12();break;
simontruelove 6:f7028034aabb 134 case 4:Ph2();break;
simontruelove 6:f7028034aabb 135 case 5:Ph2();break;
simontruelove 6:f7028034aabb 136 case 6:Ph23();break;
simontruelove 6:f7028034aabb 137 case 7:Ph23();break;
simontruelove 6:f7028034aabb 138 case 8:Ph3();break;
simontruelove 6:f7028034aabb 139 case 9:Ph3();break;
simontruelove 6:f7028034aabb 140 case 10:Ph34();break;
simontruelove 6:f7028034aabb 141 case 11:Ph34();break;
simontruelove 6:f7028034aabb 142 case 12:Ph4();break;
simontruelove 6:f7028034aabb 143 case 13:Ph4();break;
simontruelove 6:f7028034aabb 144 case 14:Ph41();break;
simontruelove 6:f7028034aabb 145 case 15:Ph41();break;
simontruelove 5:4e5c644d5cc3 146 default:break;
simontruelove 4:3aedc9246ae4 147 }
simontruelove 4:3aedc9246ae4 148
simontruelove 7:b8de1529c7fc 149 if(wheel.getWhoop()==1)
simontruelove 4:3aedc9246ae4 150 {
simontruelove 5:4e5c644d5cc3 151 RPM();
simontruelove 7:b8de1529c7fc 152 //VelocityLoop();
simontruelove 4:3aedc9246ae4 153 }
simontruelove 4:3aedc9246ae4 154 }
simontruelove 4:3aedc9246ae4 155
simontruelove 7:b8de1529c7fc 156 while(wheel.getRevolutions()>1 && wheel.getPulses()<1 && (led1==1))
simontruelove 4:3aedc9246ae4 157 {
simontruelove 4:3aedc9246ae4 158 GetChar();
simontruelove 6:f7028034aabb 159 StateB = (800+wheel.getPulses()+StateA+AdjCW)%16;
simontruelove 6:f7028034aabb 160 //pc.printf("StateA= %i\r", StateA);
simontruelove 4:3aedc9246ae4 161 //pc.printf("2 StateB= %i, Pulses= %i, Revs= %i\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 3:4249dbdf7ed3 162
simontruelove 3:4249dbdf7ed3 163 switch(StateB)
simontruelove 3:4249dbdf7ed3 164 {
simontruelove 6:f7028034aabb 165 case 0:Ph1();break;
simontruelove 6:f7028034aabb 166 case 1:Ph1();break;
simontruelove 6:f7028034aabb 167 case 2:Ph12();break;
simontruelove 6:f7028034aabb 168 case 3:Ph12();break;
simontruelove 6:f7028034aabb 169 case 4:Ph2();break;
simontruelove 6:f7028034aabb 170 case 5:Ph2();break;
simontruelove 6:f7028034aabb 171 case 6:Ph23();break;
simontruelove 6:f7028034aabb 172 case 7:Ph23();break;
simontruelove 6:f7028034aabb 173 case 8:Ph3();break;
simontruelove 6:f7028034aabb 174 case 9:Ph3();break;
simontruelove 6:f7028034aabb 175 case 10:Ph34();break;
simontruelove 6:f7028034aabb 176 case 11:Ph34();break;
simontruelove 6:f7028034aabb 177 case 12:Ph4();break;
simontruelove 6:f7028034aabb 178 case 13:Ph4();break;
simontruelove 6:f7028034aabb 179 case 14:Ph41();break;
simontruelove 6:f7028034aabb 180 case 15:Ph41();break;
simontruelove 5:4e5c644d5cc3 181 default:break;
simontruelove 3:4249dbdf7ed3 182 }
simontruelove 3:4249dbdf7ed3 183
simontruelove 7:b8de1529c7fc 184 if(wheel.getWhoop()==1)
simontruelove 5:4e5c644d5cc3 185 {
simontruelove 5:4e5c644d5cc3 186 RPM();
simontruelove 7:b8de1529c7fc 187 //VelocityLoop();
simontruelove 5:4e5c644d5cc3 188 }
simontruelove 5:4e5c644d5cc3 189 }
simontruelove 7:b8de1529c7fc 190 while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led2==1))
simontruelove 4:3aedc9246ae4 191 {
simontruelove 5:4e5c644d5cc3 192 GetChar();
simontruelove 6:f7028034aabb 193 //StateB = (800+wheel.getPulses())%16;
simontruelove 6:f7028034aabb 194 StateB = (800+wheel.getPulses()+StateA+AdjACW)%16;
simontruelove 6:f7028034aabb 195 //pc.printf("StateA= %i\r", StateA);
simontruelove 5:4e5c644d5cc3 196 //pc.printf("3 StateB= %i, Pulses= %i, Revs= %i \r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 5:4e5c644d5cc3 197 switch(StateB)
simontruelove 5:4e5c644d5cc3 198 {
simontruelove 6:f7028034aabb 199 case 15:Ph41();break;
simontruelove 6:f7028034aabb 200 case 14:Ph41();break;
simontruelove 6:f7028034aabb 201 case 13:Ph4();break;
simontruelove 6:f7028034aabb 202 case 12:Ph4();break;
simontruelove 6:f7028034aabb 203 case 11:Ph34();break;
simontruelove 6:f7028034aabb 204 case 10:Ph34();break;
simontruelove 6:f7028034aabb 205 case 9:Ph3();break;
simontruelove 6:f7028034aabb 206 case 8:Ph3();break;
simontruelove 6:f7028034aabb 207 case 7:Ph23();break;
simontruelove 6:f7028034aabb 208 case 6:Ph23();break;
simontruelove 6:f7028034aabb 209 case 5:Ph2();break;
simontruelove 6:f7028034aabb 210 case 4:Ph2();break;
simontruelove 6:f7028034aabb 211 case 3:Ph12();break;
simontruelove 6:f7028034aabb 212 case 2:Ph12();break;
simontruelove 6:f7028034aabb 213 case 1:Ph1();break;
simontruelove 6:f7028034aabb 214 case 0:Ph1();break;
simontruelove 5:4e5c644d5cc3 215 default:break;
simontruelove 5:4e5c644d5cc3 216 }
simontruelove 6:f7028034aabb 217
simontruelove 7:b8de1529c7fc 218 if(wheel.getWhoop()==1)
simontruelove 5:4e5c644d5cc3 219 {
simontruelove 5:4e5c644d5cc3 220 RPM();
simontruelove 7:b8de1529c7fc 221 //VelocityLoop();
simontruelove 5:4e5c644d5cc3 222 }
simontruelove 3:4249dbdf7ed3 223 }
simontruelove 7:b8de1529c7fc 224 while((wheel.getRevolutions()>1) && (wheel.getPulses()<1) && (led2==1))
simontruelove 5:4e5c644d5cc3 225 {
simontruelove 5:4e5c644d5cc3 226 GetChar();
simontruelove 6:f7028034aabb 227 StateB = (800+wheel.getPulses()+StateA+AdjACW)%16;
simontruelove 6:f7028034aabb 228 //pc.printf("StateA= %i\r", StateA);
simontruelove 5:4e5c644d5cc3 229 //pc.printf("4 StateB= %i, Pulses= %i, Revs= %i \r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 5:4e5c644d5cc3 230 switch(StateB)
simontruelove 5:4e5c644d5cc3 231 {
simontruelove 6:f7028034aabb 232 case 15:Ph41();break;
simontruelove 6:f7028034aabb 233 case 14:Ph41();break;
simontruelove 6:f7028034aabb 234 case 13:Ph4();break;
simontruelove 6:f7028034aabb 235 case 12:Ph4();break;
simontruelove 6:f7028034aabb 236 case 11:Ph34();break;
simontruelove 6:f7028034aabb 237 case 10:Ph34();break;
simontruelove 6:f7028034aabb 238 case 9:Ph3();break;
simontruelove 6:f7028034aabb 239 case 8:Ph3();break;
simontruelove 6:f7028034aabb 240 case 7:Ph23();break;
simontruelove 6:f7028034aabb 241 case 6:Ph23();break;
simontruelove 6:f7028034aabb 242 case 5:Ph2();break;
simontruelove 6:f7028034aabb 243 case 4:Ph2();break;
simontruelove 6:f7028034aabb 244 case 3:Ph12();break;
simontruelove 6:f7028034aabb 245 case 2:Ph12();break;
simontruelove 6:f7028034aabb 246 case 1:Ph1();break;
simontruelove 6:f7028034aabb 247 case 0:Ph1();break;
simontruelove 5:4e5c644d5cc3 248 default:break;
simontruelove 5:4e5c644d5cc3 249 }
simontruelove 7:b8de1529c7fc 250 if(wheel.getWhoop()==1)
simontruelove 5:4e5c644d5cc3 251 {
simontruelove 5:4e5c644d5cc3 252 RPM();
simontruelove 7:b8de1529c7fc 253 //VelocityLoop();
simontruelove 5:4e5c644d5cc3 254 }
simontruelove 5:4e5c644d5cc3 255 }
simontruelove 1:0191658b6ff4 256 }
simontruelove 1:0191658b6ff4 257 }
simontruelove 1:0191658b6ff4 258 void StepCW(void) //Square wave switching
simontruelove 1:0191658b6ff4 259 {
simontruelove 1:0191658b6ff4 260 Ph1();
simontruelove 3:4249dbdf7ed3 261 wait(x);
simontruelove 3:4249dbdf7ed3 262 Ph12();
simontruelove 1:0191658b6ff4 263 wait(y);
simontruelove 1:0191658b6ff4 264 Ph2();
simontruelove 3:4249dbdf7ed3 265 wait(x);
simontruelove 3:4249dbdf7ed3 266 Ph23();
simontruelove 1:0191658b6ff4 267 wait(y);
simontruelove 1:0191658b6ff4 268 Ph3();
simontruelove 3:4249dbdf7ed3 269 wait(x);
simontruelove 3:4249dbdf7ed3 270 Ph34();
simontruelove 1:0191658b6ff4 271 wait(y);
simontruelove 1:0191658b6ff4 272 Ph4();
simontruelove 3:4249dbdf7ed3 273 wait(x);
simontruelove 3:4249dbdf7ed3 274 Ph41();
simontruelove 3:4249dbdf7ed3 275 wait(y);
simontruelove 1:0191658b6ff4 276 }
simontruelove 1:0191658b6ff4 277
simontruelove 1:0191658b6ff4 278 void Ph1(void)
simontruelove 1:0191658b6ff4 279 {
simontruelove 7:b8de1529c7fc 280 Phase1.write(duty);
simontruelove 7:b8de1529c7fc 281 Phase2.write(0);
simontruelove 7:b8de1529c7fc 282 Phase3.write(0);
simontruelove 7:b8de1529c7fc 283 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 284 //wait(x);
simontruelove 3:4249dbdf7ed3 285 //pc.printf("Phase 1 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 286 }
simontruelove 3:4249dbdf7ed3 287
simontruelove 3:4249dbdf7ed3 288 void Ph12 (void)
simontruelove 3:4249dbdf7ed3 289 {
simontruelove 7:b8de1529c7fc 290 Phase1.write(duty);
simontruelove 7:b8de1529c7fc 291 Phase2.write(duty);
simontruelove 7:b8de1529c7fc 292 Phase3.write(0);
simontruelove 7:b8de1529c7fc 293 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 294 //wait(y);
simontruelove 1:0191658b6ff4 295 }
simontruelove 1:0191658b6ff4 296
simontruelove 1:0191658b6ff4 297 void Ph2(void)
simontruelove 1:0191658b6ff4 298 {
simontruelove 7:b8de1529c7fc 299 Phase1.write(0);
simontruelove 7:b8de1529c7fc 300 Phase2.write(duty);
simontruelove 7:b8de1529c7fc 301 Phase3.write(0);
simontruelove 7:b8de1529c7fc 302 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 303 //wait(x);
simontruelove 3:4249dbdf7ed3 304 //pc.printf("Phase 2 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 305 }
simontruelove 3:4249dbdf7ed3 306
simontruelove 3:4249dbdf7ed3 307 void Ph23 (void)
simontruelove 3:4249dbdf7ed3 308 {
simontruelove 7:b8de1529c7fc 309 Phase1.write(0);
simontruelove 7:b8de1529c7fc 310 Phase2.write(duty);
simontruelove 7:b8de1529c7fc 311 Phase3.write(duty);
simontruelove 7:b8de1529c7fc 312 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 313 //wait(y);
simontruelove 1:0191658b6ff4 314 }
simontruelove 0:634dd505dace 315
simontruelove 1:0191658b6ff4 316 void Ph3(void)
simontruelove 1:0191658b6ff4 317 {
simontruelove 7:b8de1529c7fc 318 Phase1.write(0);
simontruelove 7:b8de1529c7fc 319 Phase2.write(0);
simontruelove 7:b8de1529c7fc 320 Phase3.write(duty);
simontruelove 7:b8de1529c7fc 321 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 322 //wait(x);
simontruelove 3:4249dbdf7ed3 323 //pc.printf("Phase 3 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 324 }
simontruelove 3:4249dbdf7ed3 325 void Ph34 (void)
simontruelove 3:4249dbdf7ed3 326 {
simontruelove 7:b8de1529c7fc 327 Phase1.write(0);
simontruelove 7:b8de1529c7fc 328 Phase2.write(0);
simontruelove 7:b8de1529c7fc 329 Phase3.write(duty);
simontruelove 7:b8de1529c7fc 330 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 331 //wait(y);
simontruelove 1:0191658b6ff4 332 }
simontruelove 1:0191658b6ff4 333
simontruelove 1:0191658b6ff4 334 void Ph4(void)
simontruelove 1:0191658b6ff4 335 {
simontruelove 7:b8de1529c7fc 336 Phase1.write(0);
simontruelove 7:b8de1529c7fc 337 Phase2.write(0);
simontruelove 7:b8de1529c7fc 338 Phase3.write(0);
simontruelove 7:b8de1529c7fc 339 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 340 //wait(x);
simontruelove 3:4249dbdf7ed3 341 //pc.printf("Phase 4 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 342 }
simontruelove 3:4249dbdf7ed3 343
simontruelove 3:4249dbdf7ed3 344 void Ph41 (void)
simontruelove 3:4249dbdf7ed3 345 {
simontruelove 7:b8de1529c7fc 346 Phase1.write(duty);
simontruelove 7:b8de1529c7fc 347 Phase2.write(0);
simontruelove 7:b8de1529c7fc 348 Phase3.write(0);
simontruelove 7:b8de1529c7fc 349 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 350 //wait(y);
simontruelove 3:4249dbdf7ed3 351 }
simontruelove 3:4249dbdf7ed3 352
simontruelove 3:4249dbdf7ed3 353 void Initialisation (void)
simontruelove 3:4249dbdf7ed3 354 {
simontruelove 7:b8de1529c7fc 355 Phase1.write(0);
simontruelove 7:b8de1529c7fc 356 Phase2.write(0);
simontruelove 7:b8de1529c7fc 357 Phase3.write(0);
simontruelove 7:b8de1529c7fc 358 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 359 led1 = 0;
simontruelove 3:4249dbdf7ed3 360 led2 = 0;
simontruelove 3:4249dbdf7ed3 361 led3 = 0;
simontruelove 3:4249dbdf7ed3 362 led4 = 0;
simontruelove 3:4249dbdf7ed3 363 wheel.ResetYay();
simontruelove 4:3aedc9246ae4 364 }
simontruelove 4:3aedc9246ae4 365
simontruelove 4:3aedc9246ae4 366 void GetChar (void)
simontruelove 4:3aedc9246ae4 367 { if (pc.readable())
simontruelove 4:3aedc9246ae4 368 {
simontruelove 4:3aedc9246ae4 369 c = pc.getc();
simontruelove 4:3aedc9246ae4 370 if(c == 'z')
simontruelove 4:3aedc9246ae4 371 {
simontruelove 4:3aedc9246ae4 372 led1 = !led1;
simontruelove 4:3aedc9246ae4 373 led2 = 0;
simontruelove 4:3aedc9246ae4 374 }
simontruelove 4:3aedc9246ae4 375 if(c == 'x')
simontruelove 4:3aedc9246ae4 376 {
simontruelove 4:3aedc9246ae4 377 led1 = 0;
simontruelove 4:3aedc9246ae4 378 led2 = !led2 ;
simontruelove 4:3aedc9246ae4 379 }
simontruelove 7:b8de1529c7fc 380 if(c == 'q')
simontruelove 7:b8de1529c7fc 381 {
simontruelove 7:b8de1529c7fc 382 SetPoint=SetPoint+10;
simontruelove 7:b8de1529c7fc 383 }
simontruelove 7:b8de1529c7fc 384 if(c == 'a')
simontruelove 7:b8de1529c7fc 385 {
simontruelove 7:b8de1529c7fc 386 SetPoint=SetPoint-10;
simontruelove 7:b8de1529c7fc 387 //if (Setpoint <100)
simontruelove 7:b8de1529c7fc 388 //{
simontruelove 7:b8de1529c7fc 389 // SetPoint = 100;
simontruelove 7:b8de1529c7fc 390 // }
simontruelove 7:b8de1529c7fc 391 }
simontruelove 4:3aedc9246ae4 392 }
simontruelove 5:4e5c644d5cc3 393 }
simontruelove 5:4e5c644d5cc3 394
simontruelove 5:4e5c644d5cc3 395 void RPM (void)
simontruelove 5:4e5c644d5cc3 396 {
simontruelove 7:b8de1529c7fc 397 wheel.ResetWhoop();
simontruelove 5:4e5c644d5cc3 398 TimePerClick = (t.read_us());
simontruelove 5:4e5c644d5cc3 399 t.reset();
simontruelove 7:b8de1529c7fc 400 TimePerRev = TimePerClick * (800/z);
simontruelove 5:4e5c644d5cc3 401 TimePerRev = TimePerRev / 1000;
simontruelove 5:4e5c644d5cc3 402 RPS = 10000000 / TimePerRev;
simontruelove 5:4e5c644d5cc3 403 rpm = (RPS * 60)/10000;
simontruelove 7:b8de1529c7fc 404 Aout=((0.298*rpm)/1000); // for 500 rpm (0.30303*500/1000)*3.3V = 0.500V
simontruelove 7:b8de1529c7fc 405 //pc.printf("rpm = %i\n\r", rpm);
simontruelove 6:f7028034aabb 406 //pc.printf("StateA= %i, StateB= %i, StateC= %i, Pulses = %i\n\r", StateA, StateB, StateC, wheel.getPulses());
simontruelove 7:b8de1529c7fc 407 }
simontruelove 7:b8de1529c7fc 408
simontruelove 7:b8de1529c7fc 409 void VelocityLoop (void)
simontruelove 7:b8de1529c7fc 410 {
simontruelove 7:b8de1529c7fc 411 diff = SetPoint - rpm;
simontruelove 7:b8de1529c7fc 412 duty = duty + (diff*0.00005);
simontruelove 7:b8de1529c7fc 413 if (duty > 0.9)
simontruelove 7:b8de1529c7fc 414 {
simontruelove 7:b8de1529c7fc 415 duty = 0.9;
simontruelove 7:b8de1529c7fc 416 }
simontruelove 7:b8de1529c7fc 417 if (duty <0.1)
simontruelove 7:b8de1529c7fc 418 {
simontruelove 7:b8de1529c7fc 419 duty = 0.1;
simontruelove 7:b8de1529c7fc 420 }
simontruelove 7:b8de1529c7fc 421 pc.printf("duty = %f, SetPoint = %i, rpm = %i\n\r", duty, SetPoint, rpm);
simontruelove 3:4249dbdf7ed3 422 }