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

Committer:
simontruelove
Date:
Mon Nov 19 15:30:08 2018 +0000
Revision:
8:2e690f407ec6
Parent:
7:b8de1529c7fc
Child:
9:061600a6c750
FastPWM

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