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@11:74eeb8871fe6, 2018-12-17 (annotated)

Committer:
simontruelove
Date:
Mon Dec 17 08:37:21 2018 +0000
Revision:
11:74eeb8871fe6
Parent:
10:808cb9052f14
Child:
12:cbea987a3ec4
Thermocouple reading incorrect when motor running. Temperature changes as motor duty changes

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 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 11:74eeb8871fe6 17 void ReadKType(void);
simontruelove 1:0191658b6ff4 18
simontruelove 10:808cb9052f14 19 Serial pc(USBTX, USBRX); // tx, rx - set up the Terraterm input from mbed
simontruelove 1:0191658b6ff4 20
simontruelove 10:808cb9052f14 21 QEI wheel(p5, p6, p8, 800, QEI::X4_ENCODING); //code for quadrature encoder see QEI.h
simontruelove 3:4249dbdf7ed3 22
simontruelove 10:808cb9052f14 23 Timer t; //timer used in RPM
simontruelove 3:4249dbdf7ed3 24
simontruelove 11:74eeb8871fe6 25 PwmOut Phase1 (p21); //Pin and LED set up - originally standard pins but changed to PWM to enable speed control
simontruelove 11:74eeb8871fe6 26 PwmOut Phase2 (p23);
simontruelove 11:74eeb8871fe6 27 PwmOut Phase3 (p24);
simontruelove 11:74eeb8871fe6 28 PwmOut Phase4 (p25);
simontruelove 1:0191658b6ff4 29
simontruelove 10:808cb9052f14 30 AnalogOut Aout(p18); //Used with multimeter to give a speed indicator 1mV = 1RPM
simontruelove 6:f7028034aabb 31
simontruelove 10:808cb9052f14 32 //DigitalIn Button1 (p11); //not used
simontruelove 10:808cb9052f14 33 //DigitalIn Button2 (p12); //not used
simontruelove 2:3f95c82c26bb 34
simontruelove 10:808cb9052f14 35 DigitalOut led1(LED1); //LEDs used to as very basic memmory for controlling the state machines
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 10:808cb9052f14 40 DigitalOut SerialClock (p12); //ReadKType
simontruelove 10:808cb9052f14 41 DigitalIn DOut (p13); //ReadKType
simontruelove 10:808cb9052f14 42 DigitalOut cs1 (p14); //ReadKType
simontruelove 10:808cb9052f14 43
simontruelove 10:808cb9052f14 44 int StateA = 0; //State for first 2 revolutions (calibration of the index)
simontruelove 10:808cb9052f14 45 int StateB = 0; //All state machines after calibration use this state
simontruelove 6:f7028034aabb 46 //int StateC = 0;
simontruelove 10:808cb9052f14 47 int AdjCW = 0; //CW offset to adjust phase firing to give the fastest speed = used to calculate stateB
simontruelove 10:808cb9052f14 48 int AdjACW = 3; //ACW offset to adjust phase firing to give the fastest speed = used to calculate stateB
simontruelove 10:808cb9052f14 49 int TimePerClick = 0; //for calc of RPM
simontruelove 10:808cb9052f14 50 int TimePerRev = 0; //for calc of RPM
simontruelove 10:808cb9052f14 51 int RPS = 0; //for calc of RPMl;
simontruelove 10:808cb9052f14 52 int rpm = 0; //for calc of RPM
simontruelove 10:808cb9052f14 53 int SetPoint = 1000; //for adjusting the speed
simontruelove 10:808cb9052f14 54 int z = 80; //TimePerRev = TimePerClick * (800/z); 800 pulses per rev, PulseCount2_==80 for wheel.getwhoop_ flag. i.e. 10 points per reoluition for RPM calc.
simontruelove 11:74eeb8871fe6 55 int T = 80; //Motor temp limit
simontruelove 11:74eeb8871fe6 56
simontruelove 10:808cb9052f14 57 char c; //keyboard cotrol GetChar
simontruelove 4:3aedc9246ae4 58
simontruelove 11:74eeb8871fe6 59 float duty = 1; //velocity loop: 1 = fastest, 0.96 = slowest. Below 0.96 the motor will not operate.
simontruelove 10:808cb9052f14 60 float diff = 0.0; //Velocity loop: diff = SetPoint - rpm;
simontruelove 10:808cb9052f14 61 float x=0.1; //x=time of square wave when 1 phase energised,
simontruelove 10:808cb9052f14 62 float y=0.04; //y=time of square wave when 2 phases energised
simontruelove 10:808cb9052f14 63 float temp = 0; //ReadKType
simontruelove 1:0191658b6ff4 64
simontruelove 1:0191658b6ff4 65 int main(void)
simontruelove 3:4249dbdf7ed3 66 {
simontruelove 10:808cb9052f14 67 pc.baud(230400); //Set fastest baud rate
simontruelove 10:808cb9052f14 68 Phase1.period(0.00002); //period of 0.000002 = 2 microseconds (50kHz). Good balance of low and high speed performance.
simontruelove 9:061600a6c750 69 Phase2.period(0.00002);
simontruelove 9:061600a6c750 70 Phase3.period(0.00002);
simontruelove 9:061600a6c750 71 Phase4.period(0.00002);
simontruelove 3:4249dbdf7ed3 72 StepCW();
simontruelove 3:4249dbdf7ed3 73 Initialisation();
simontruelove 3:4249dbdf7ed3 74 wait(0.1);
simontruelove 3:4249dbdf7ed3 75 t.start();
simontruelove 11:74eeb8871fe6 76 SerialClock = 0;
simontruelove 10:808cb9052f14 77
simontruelove 10:808cb9052f14 78 while(wheel.getRevolutions()<2) //Index Calibration
simontruelove 6:f7028034aabb 79 {
simontruelove 5:4e5c644d5cc3 80 switch(StateA)
simontruelove 3:4249dbdf7ed3 81 {
simontruelove 6:f7028034aabb 82 case 0:Ph1();break;
simontruelove 6:f7028034aabb 83 case 1:Ph1();break;
simontruelove 6:f7028034aabb 84 case 2:Ph12();break;
simontruelove 6:f7028034aabb 85 case 3:Ph12();break;
simontruelove 6:f7028034aabb 86 case 4:Ph2();break;
simontruelove 6:f7028034aabb 87 case 5:Ph2();break;
simontruelove 6:f7028034aabb 88 case 6:Ph23();break;
simontruelove 6:f7028034aabb 89 case 7:Ph23();break;
simontruelove 6:f7028034aabb 90 case 8:Ph3();break;
simontruelove 6:f7028034aabb 91 case 9:Ph3();break;
simontruelove 6:f7028034aabb 92 case 10:Ph34();break;
simontruelove 6:f7028034aabb 93 case 11:Ph34();break;
simontruelove 6:f7028034aabb 94 case 12:Ph4();break;
simontruelove 6:f7028034aabb 95 case 13:Ph4();break;
simontruelove 6:f7028034aabb 96 case 14:Ph41();break;
simontruelove 6:f7028034aabb 97 case 15:Ph41();break;
simontruelove 5:4e5c644d5cc3 98 default:break;
simontruelove 3:4249dbdf7ed3 99 }
simontruelove 3:4249dbdf7ed3 100
simontruelove 10:808cb9052f14 101 if(wheel.getYay()==1) //PulseCount_==1, yay_=1;
simontruelove 3:4249dbdf7ed3 102 {
simontruelove 5:4e5c644d5cc3 103 StateA++;
simontruelove 5:4e5c644d5cc3 104 wheel.ResetYay();
simontruelove 6:f7028034aabb 105 if (StateA>15)
simontruelove 3:4249dbdf7ed3 106 {
simontruelove 5:4e5c644d5cc3 107 StateA=0;
simontruelove 3:4249dbdf7ed3 108 }
simontruelove 3:4249dbdf7ed3 109 }
simontruelove 5:4e5c644d5cc3 110 }
simontruelove 3:4249dbdf7ed3 111
simontruelove 5:4e5c644d5cc3 112 while(1)
simontruelove 5:4e5c644d5cc3 113 {
simontruelove 10:808cb9052f14 114 while((led1 == 0) && (led2 == 0)) //If no command to operate
simontruelove 4:3aedc9246ae4 115 {
simontruelove 7:b8de1529c7fc 116 Aout = 0;
simontruelove 10:808cb9052f14 117 rpm = 0;
simontruelove 9:061600a6c750 118 Phase1.write(0);
simontruelove 9:061600a6c750 119 Phase2.write(0);
simontruelove 9:061600a6c750 120 Phase3.write(0);
simontruelove 9:061600a6c750 121 Phase4.write(0);
simontruelove 10:808cb9052f14 122 GetChar();
simontruelove 11:74eeb8871fe6 123 ReadKType();
simontruelove 6:f7028034aabb 124 //StateB = wheel.getPulses()%16;
simontruelove 6:f7028034aabb 125 //StateC = (800+wheel.getPulses()+StateA+AdjCW)%16;
simontruelove 6:f7028034aabb 126 //pc.printf("StateA= %i, StateB= %i, StateC= %i, Pulses = %i\n\r", StateA, StateB, StateC, wheel.getPulses());
simontruelove 5:4e5c644d5cc3 127 //pc.printf("0 StateB= %i, Pulses= %i, Revs= %i\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 4:3aedc9246ae4 128 }
simontruelove 11:74eeb8871fe6 129 while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led1==1) && (temp<T)) //After Calibration, Prev CW movement, CW command
simontruelove 3:4249dbdf7ed3 130 {
simontruelove 4:3aedc9246ae4 131 GetChar();
simontruelove 6:f7028034aabb 132 StateB = (wheel.getPulses()+StateA+AdjCW)%16;
simontruelove 7:b8de1529c7fc 133 //pc.printf("rpm = %i, Whoop = %i\n\r", rpm, wheel.getWhoop());
simontruelove 6:f7028034aabb 134 //pc.printf("StateB= %i\n\r", StateB);
simontruelove 4:3aedc9246ae4 135 //pc.printf("1 StateB= %i, Pulses= %i, Revs= %i\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 4:3aedc9246ae4 136
simontruelove 4:3aedc9246ae4 137 switch(StateB)
simontruelove 4:3aedc9246ae4 138 {
simontruelove 6:f7028034aabb 139 case 0:Ph1();break;
simontruelove 6:f7028034aabb 140 case 1:Ph1();break;
simontruelove 6:f7028034aabb 141 case 2:Ph12();break;
simontruelove 6:f7028034aabb 142 case 3:Ph12();break;
simontruelove 6:f7028034aabb 143 case 4:Ph2();break;
simontruelove 6:f7028034aabb 144 case 5:Ph2();break;
simontruelove 6:f7028034aabb 145 case 6:Ph23();break;
simontruelove 6:f7028034aabb 146 case 7:Ph23();break;
simontruelove 6:f7028034aabb 147 case 8:Ph3();break;
simontruelove 6:f7028034aabb 148 case 9:Ph3();break;
simontruelove 6:f7028034aabb 149 case 10:Ph34();break;
simontruelove 6:f7028034aabb 150 case 11:Ph34();break;
simontruelove 6:f7028034aabb 151 case 12:Ph4();break;
simontruelove 6:f7028034aabb 152 case 13:Ph4();break;
simontruelove 6:f7028034aabb 153 case 14:Ph41();break;
simontruelove 6:f7028034aabb 154 case 15:Ph41();break;
simontruelove 5:4e5c644d5cc3 155 default:break;
simontruelove 4:3aedc9246ae4 156 }
simontruelove 4:3aedc9246ae4 157
simontruelove 10:808cb9052f14 158 if(wheel.getWhoop()==1) //PulseCount2_==80, whoop_=1;
simontruelove 4:3aedc9246ae4 159 {
simontruelove 5:4e5c644d5cc3 160 RPM();
simontruelove 9:061600a6c750 161 VelocityLoop();
simontruelove 11:74eeb8871fe6 162 }
simontruelove 4:3aedc9246ae4 163 }
simontruelove 4:3aedc9246ae4 164
simontruelove 11:74eeb8871fe6 165 while(wheel.getRevolutions()>1 && wheel.getPulses()<1 && (led1==1) && (temp<T)) //After Calibration, Prev ACW movement, CW command
simontruelove 4:3aedc9246ae4 166 {
simontruelove 4:3aedc9246ae4 167 GetChar();
simontruelove 6:f7028034aabb 168 StateB = (800+wheel.getPulses()+StateA+AdjCW)%16;
simontruelove 6:f7028034aabb 169 //pc.printf("StateA= %i\r", StateA);
simontruelove 4:3aedc9246ae4 170 //pc.printf("2 StateB= %i, Pulses= %i, Revs= %i\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 3:4249dbdf7ed3 171
simontruelove 3:4249dbdf7ed3 172 switch(StateB)
simontruelove 3:4249dbdf7ed3 173 {
simontruelove 6:f7028034aabb 174 case 0:Ph1();break;
simontruelove 6:f7028034aabb 175 case 1:Ph1();break;
simontruelove 6:f7028034aabb 176 case 2:Ph12();break;
simontruelove 6:f7028034aabb 177 case 3:Ph12();break;
simontruelove 6:f7028034aabb 178 case 4:Ph2();break;
simontruelove 6:f7028034aabb 179 case 5:Ph2();break;
simontruelove 6:f7028034aabb 180 case 6:Ph23();break;
simontruelove 6:f7028034aabb 181 case 7:Ph23();break;
simontruelove 6:f7028034aabb 182 case 8:Ph3();break;
simontruelove 6:f7028034aabb 183 case 9:Ph3();break;
simontruelove 6:f7028034aabb 184 case 10:Ph34();break;
simontruelove 6:f7028034aabb 185 case 11:Ph34();break;
simontruelove 6:f7028034aabb 186 case 12:Ph4();break;
simontruelove 6:f7028034aabb 187 case 13:Ph4();break;
simontruelove 6:f7028034aabb 188 case 14:Ph41();break;
simontruelove 6:f7028034aabb 189 case 15:Ph41();break;
simontruelove 5:4e5c644d5cc3 190 default:break;
simontruelove 3:4249dbdf7ed3 191 }
simontruelove 3:4249dbdf7ed3 192
simontruelove 10:808cb9052f14 193 if(wheel.getWhoop()==1) //PulseCount2_==80, whoop_=1;
simontruelove 5:4e5c644d5cc3 194 {
simontruelove 5:4e5c644d5cc3 195 RPM();
simontruelove 9:061600a6c750 196 VelocityLoop();
simontruelove 11:74eeb8871fe6 197 }
simontruelove 5:4e5c644d5cc3 198 }
simontruelove 11:74eeb8871fe6 199 while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led2==1) && (temp<T)) //After Calibration, Prev CW movement, ACW command
simontruelove 4:3aedc9246ae4 200 {
simontruelove 5:4e5c644d5cc3 201 GetChar();
simontruelove 6:f7028034aabb 202 //StateB = (800+wheel.getPulses())%16;
simontruelove 6:f7028034aabb 203 StateB = (800+wheel.getPulses()+StateA+AdjACW)%16;
simontruelove 6:f7028034aabb 204 //pc.printf("StateA= %i\r", StateA);
simontruelove 5:4e5c644d5cc3 205 //pc.printf("3 StateB= %i, Pulses= %i, Revs= %i \r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 5:4e5c644d5cc3 206 switch(StateB)
simontruelove 5:4e5c644d5cc3 207 {
simontruelove 6:f7028034aabb 208 case 15:Ph41();break;
simontruelove 6:f7028034aabb 209 case 14:Ph41();break;
simontruelove 6:f7028034aabb 210 case 13:Ph4();break;
simontruelove 6:f7028034aabb 211 case 12:Ph4();break;
simontruelove 6:f7028034aabb 212 case 11:Ph34();break;
simontruelove 6:f7028034aabb 213 case 10:Ph34();break;
simontruelove 6:f7028034aabb 214 case 9:Ph3();break;
simontruelove 6:f7028034aabb 215 case 8:Ph3();break;
simontruelove 6:f7028034aabb 216 case 7:Ph23();break;
simontruelove 6:f7028034aabb 217 case 6:Ph23();break;
simontruelove 6:f7028034aabb 218 case 5:Ph2();break;
simontruelove 6:f7028034aabb 219 case 4:Ph2();break;
simontruelove 6:f7028034aabb 220 case 3:Ph12();break;
simontruelove 6:f7028034aabb 221 case 2:Ph12();break;
simontruelove 6:f7028034aabb 222 case 1:Ph1();break;
simontruelove 6:f7028034aabb 223 case 0:Ph1();break;
simontruelove 5:4e5c644d5cc3 224 default:break;
simontruelove 5:4e5c644d5cc3 225 }
simontruelove 6:f7028034aabb 226
simontruelove 10:808cb9052f14 227 if(wheel.getWhoop()==1) //PulseCount2_==80, whoop_=1;
simontruelove 5:4e5c644d5cc3 228 {
simontruelove 5:4e5c644d5cc3 229 RPM();
simontruelove 9:061600a6c750 230 VelocityLoop();
simontruelove 5:4e5c644d5cc3 231 }
simontruelove 3:4249dbdf7ed3 232 }
simontruelove 11:74eeb8871fe6 233 while((wheel.getRevolutions()>1) && (wheel.getPulses()<1) && (led2==1) &&(temp<T)) //After Calibration, Prev ACW movement, ACW command
simontruelove 5:4e5c644d5cc3 234 {
simontruelove 5:4e5c644d5cc3 235 GetChar();
simontruelove 6:f7028034aabb 236 StateB = (800+wheel.getPulses()+StateA+AdjACW)%16;
simontruelove 6:f7028034aabb 237 //pc.printf("StateA= %i\r", StateA);
simontruelove 5:4e5c644d5cc3 238 //pc.printf("4 StateB= %i, Pulses= %i, Revs= %i \r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 5:4e5c644d5cc3 239 switch(StateB)
simontruelove 5:4e5c644d5cc3 240 {
simontruelove 6:f7028034aabb 241 case 15:Ph41();break;
simontruelove 6:f7028034aabb 242 case 14:Ph41();break;
simontruelove 6:f7028034aabb 243 case 13:Ph4();break;
simontruelove 6:f7028034aabb 244 case 12:Ph4();break;
simontruelove 6:f7028034aabb 245 case 11:Ph34();break;
simontruelove 6:f7028034aabb 246 case 10:Ph34();break;
simontruelove 6:f7028034aabb 247 case 9:Ph3();break;
simontruelove 6:f7028034aabb 248 case 8:Ph3();break;
simontruelove 6:f7028034aabb 249 case 7:Ph23();break;
simontruelove 6:f7028034aabb 250 case 6:Ph23();break;
simontruelove 6:f7028034aabb 251 case 5:Ph2();break;
simontruelove 6:f7028034aabb 252 case 4:Ph2();break;
simontruelove 6:f7028034aabb 253 case 3:Ph12();break;
simontruelove 6:f7028034aabb 254 case 2:Ph12();break;
simontruelove 6:f7028034aabb 255 case 1:Ph1();break;
simontruelove 6:f7028034aabb 256 case 0:Ph1();break;
simontruelove 5:4e5c644d5cc3 257 default:break;
simontruelove 5:4e5c644d5cc3 258 }
simontruelove 10:808cb9052f14 259 if(wheel.getWhoop()==1) //PulseCount2_==80, whoop_=1;
simontruelove 5:4e5c644d5cc3 260 {
simontruelove 5:4e5c644d5cc3 261 RPM();
simontruelove 9:061600a6c750 262 VelocityLoop();
simontruelove 11:74eeb8871fe6 263 }
simontruelove 5:4e5c644d5cc3 264 }
simontruelove 11:74eeb8871fe6 265 while(temp>(T-1))
simontruelove 11:74eeb8871fe6 266 {
simontruelove 11:74eeb8871fe6 267 Initialisation();
simontruelove 11:74eeb8871fe6 268 pc.printf("Motor Over Temp\n\r");
simontruelove 11:74eeb8871fe6 269 while(1)
simontruelove 11:74eeb8871fe6 270 {
simontruelove 11:74eeb8871fe6 271 ReadKType();
simontruelove 11:74eeb8871fe6 272 pc.printf("%f\r",temp);
simontruelove 11:74eeb8871fe6 273 wait(1);
simontruelove 11:74eeb8871fe6 274 if(temp<T-20)
simontruelove 11:74eeb8871fe6 275 {
simontruelove 11:74eeb8871fe6 276 pc.printf("Motor Back Online\n\r");
simontruelove 11:74eeb8871fe6 277 break;
simontruelove 11:74eeb8871fe6 278 }
simontruelove 11:74eeb8871fe6 279 }
simontruelove 11:74eeb8871fe6 280 }
simontruelove 1:0191658b6ff4 281 }
simontruelove 1:0191658b6ff4 282 }
simontruelove 1:0191658b6ff4 283 void StepCW(void) //Square wave switching
simontruelove 1:0191658b6ff4 284 {
simontruelove 1:0191658b6ff4 285 Ph1();
simontruelove 3:4249dbdf7ed3 286 wait(x);
simontruelove 3:4249dbdf7ed3 287 Ph12();
simontruelove 1:0191658b6ff4 288 wait(y);
simontruelove 1:0191658b6ff4 289 Ph2();
simontruelove 3:4249dbdf7ed3 290 wait(x);
simontruelove 3:4249dbdf7ed3 291 Ph23();
simontruelove 1:0191658b6ff4 292 wait(y);
simontruelove 1:0191658b6ff4 293 Ph3();
simontruelove 3:4249dbdf7ed3 294 wait(x);
simontruelove 3:4249dbdf7ed3 295 Ph34();
simontruelove 1:0191658b6ff4 296 wait(y);
simontruelove 1:0191658b6ff4 297 Ph4();
simontruelove 3:4249dbdf7ed3 298 wait(x);
simontruelove 3:4249dbdf7ed3 299 Ph41();
simontruelove 3:4249dbdf7ed3 300 wait(y);
simontruelove 1:0191658b6ff4 301 }
simontruelove 1:0191658b6ff4 302
simontruelove 1:0191658b6ff4 303 void Ph1(void)
simontruelove 1:0191658b6ff4 304 {
simontruelove 9:061600a6c750 305 Phase1.write(duty);
simontruelove 9:061600a6c750 306 Phase2.write(0);
simontruelove 9:061600a6c750 307 Phase3.write(0);
simontruelove 9:061600a6c750 308 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 309 //wait(x);
simontruelove 3:4249dbdf7ed3 310 //pc.printf("Phase 1 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 311 }
simontruelove 3:4249dbdf7ed3 312
simontruelove 3:4249dbdf7ed3 313 void Ph12 (void)
simontruelove 3:4249dbdf7ed3 314 {
simontruelove 9:061600a6c750 315 Phase1.write(duty);
simontruelove 9:061600a6c750 316 Phase2.write(duty);
simontruelove 9:061600a6c750 317 Phase3.write(0);
simontruelove 9:061600a6c750 318 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 319 //wait(y);
simontruelove 1:0191658b6ff4 320 }
simontruelove 1:0191658b6ff4 321
simontruelove 1:0191658b6ff4 322 void Ph2(void)
simontruelove 1:0191658b6ff4 323 {
simontruelove 9:061600a6c750 324 Phase1.write(0);
simontruelove 9:061600a6c750 325 Phase2.write(duty);
simontruelove 9:061600a6c750 326 Phase3.write(0);
simontruelove 9:061600a6c750 327 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 328 //wait(x);
simontruelove 3:4249dbdf7ed3 329 //pc.printf("Phase 2 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 330 }
simontruelove 3:4249dbdf7ed3 331
simontruelove 3:4249dbdf7ed3 332 void Ph23 (void)
simontruelove 3:4249dbdf7ed3 333 {
simontruelove 9:061600a6c750 334 Phase1.write(0);
simontruelove 9:061600a6c750 335 Phase2.write(duty);
simontruelove 9:061600a6c750 336 Phase3.write(duty);
simontruelove 9:061600a6c750 337 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 338 //wait(y);
simontruelove 1:0191658b6ff4 339 }
simontruelove 0:634dd505dace 340
simontruelove 1:0191658b6ff4 341 void Ph3(void)
simontruelove 1:0191658b6ff4 342 {
simontruelove 9:061600a6c750 343 Phase1.write(0);
simontruelove 9:061600a6c750 344 Phase2.write(0);
simontruelove 9:061600a6c750 345 Phase3.write(duty);
simontruelove 9:061600a6c750 346 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 347 //wait(x);
simontruelove 3:4249dbdf7ed3 348 //pc.printf("Phase 3 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 349 }
simontruelove 3:4249dbdf7ed3 350 void Ph34 (void)
simontruelove 3:4249dbdf7ed3 351 {
simontruelove 9:061600a6c750 352 Phase1.write(0);
simontruelove 9:061600a6c750 353 Phase2.write(0);
simontruelove 9:061600a6c750 354 Phase3.write(duty);
simontruelove 9:061600a6c750 355 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 356 //wait(y);
simontruelove 1:0191658b6ff4 357 }
simontruelove 1:0191658b6ff4 358
simontruelove 1:0191658b6ff4 359 void Ph4(void)
simontruelove 1:0191658b6ff4 360 {
simontruelove 9:061600a6c750 361 Phase1.write(0);
simontruelove 9:061600a6c750 362 Phase2.write(0);
simontruelove 9:061600a6c750 363 Phase3.write(0);
simontruelove 9:061600a6c750 364 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 365 //wait(x);
simontruelove 3:4249dbdf7ed3 366 //pc.printf("Phase 4 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 367 }
simontruelove 3:4249dbdf7ed3 368
simontruelove 3:4249dbdf7ed3 369 void Ph41 (void)
simontruelove 3:4249dbdf7ed3 370 {
simontruelove 9:061600a6c750 371 Phase1.write(duty);
simontruelove 9:061600a6c750 372 Phase2.write(0);
simontruelove 9:061600a6c750 373 Phase3.write(0);
simontruelove 9:061600a6c750 374 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 375 //wait(y);
simontruelove 3:4249dbdf7ed3 376 }
simontruelove 3:4249dbdf7ed3 377
simontruelove 10:808cb9052f14 378 void Initialisation (void) //Turn everything off
simontruelove 3:4249dbdf7ed3 379 {
simontruelove 9:061600a6c750 380 Phase1.write(0);
simontruelove 9:061600a6c750 381 Phase2.write(0);
simontruelove 9:061600a6c750 382 Phase3.write(0);
simontruelove 9:061600a6c750 383 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 384 led1 = 0;
simontruelove 3:4249dbdf7ed3 385 led2 = 0;
simontruelove 3:4249dbdf7ed3 386 led3 = 0;
simontruelove 3:4249dbdf7ed3 387 led4 = 0;
simontruelove 3:4249dbdf7ed3 388 wheel.ResetYay();
simontruelove 4:3aedc9246ae4 389 }
simontruelove 4:3aedc9246ae4 390
simontruelove 10:808cb9052f14 391 void GetChar (void) //read keyboard strikes with terraterm
simontruelove 4:3aedc9246ae4 392 { if (pc.readable())
simontruelove 4:3aedc9246ae4 393 {
simontruelove 4:3aedc9246ae4 394 c = pc.getc();
simontruelove 10:808cb9052f14 395 if(c == 'z') //turn on led1 causes CW operation
simontruelove 4:3aedc9246ae4 396 {
simontruelove 4:3aedc9246ae4 397 led1 = !led1;
simontruelove 4:3aedc9246ae4 398 led2 = 0;
simontruelove 10:808cb9052f14 399 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 4:3aedc9246ae4 400 }
simontruelove 10:808cb9052f14 401 if(c == 'x') //turn on led2 causes ACW operation
simontruelove 4:3aedc9246ae4 402 {
simontruelove 4:3aedc9246ae4 403 led1 = 0;
simontruelove 4:3aedc9246ae4 404 led2 = !led2 ;
simontruelove 10:808cb9052f14 405 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 4:3aedc9246ae4 406 }
simontruelove 10:808cb9052f14 407 if(c == 'q') //Increases setpoint used in Velocity loop
simontruelove 7:b8de1529c7fc 408 {
simontruelove 9:061600a6c750 409 //duty = duty + 0.0001;
simontruelove 10:808cb9052f14 410 SetPoint=SetPoint+10;
simontruelove 10:808cb9052f14 411 if (SetPoint >2200)
simontruelove 10:808cb9052f14 412 {
simontruelove 10:808cb9052f14 413 SetPoint = 2200;
simontruelove 10:808cb9052f14 414 }
simontruelove 10:808cb9052f14 415 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 7:b8de1529c7fc 416 }
simontruelove 10:808cb9052f14 417 if(c == 'a') //Decreases setpoint used in Velocity loop
simontruelove 7:b8de1529c7fc 418 {
simontruelove 9:061600a6c750 419 //duty = duty - 0.0001;
simontruelove 10:808cb9052f14 420 SetPoint=SetPoint-10;
simontruelove 10:808cb9052f14 421 if (SetPoint <50)
simontruelove 9:061600a6c750 422 {
simontruelove 10:808cb9052f14 423 SetPoint = 50;
simontruelove 9:061600a6c750 424 }
simontruelove 10:808cb9052f14 425 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 426 }
simontruelove 10:808cb9052f14 427 if(c== 'o')
simontruelove 10:808cb9052f14 428 {
simontruelove 10:808cb9052f14 429 AdjCW = AdjCW+1;
simontruelove 10:808cb9052f14 430 if (AdjCW >15)
simontruelove 10:808cb9052f14 431 {
simontruelove 10:808cb9052f14 432 AdjCW = 0;
simontruelove 10:808cb9052f14 433 }
simontruelove 10:808cb9052f14 434 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 435 }
simontruelove 10:808cb9052f14 436 if(c== 'k')
simontruelove 10:808cb9052f14 437 {
simontruelove 10:808cb9052f14 438 AdjCW = AdjCW-1;
simontruelove 10:808cb9052f14 439 if (AdjCW <0)
simontruelove 10:808cb9052f14 440 {
simontruelove 10:808cb9052f14 441 AdjCW = 15;
simontruelove 10:808cb9052f14 442 }
simontruelove 10:808cb9052f14 443 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 444 }
simontruelove 10:808cb9052f14 445 if(c== 'p')
simontruelove 10:808cb9052f14 446 {
simontruelove 10:808cb9052f14 447 AdjACW = AdjACW+1;
simontruelove 10:808cb9052f14 448 if (AdjACW >15)
simontruelove 10:808cb9052f14 449 {
simontruelove 10:808cb9052f14 450 AdjACW = 0;
simontruelove 10:808cb9052f14 451 }
simontruelove 10:808cb9052f14 452 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 453 }
simontruelove 10:808cb9052f14 454 if(c== 'l')
simontruelove 10:808cb9052f14 455 {
simontruelove 10:808cb9052f14 456 AdjACW = AdjACW-1;
simontruelove 10:808cb9052f14 457 if (AdjACW <0)
simontruelove 10:808cb9052f14 458 {
simontruelove 10:808cb9052f14 459 AdjACW = 15;
simontruelove 10:808cb9052f14 460 }
simontruelove 10:808cb9052f14 461 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 462 }
simontruelove 10:808cb9052f14 463 if(c=='0')
simontruelove 10:808cb9052f14 464 {
simontruelove 10:808cb9052f14 465 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 8:2e690f407ec6 466 }
simontruelove 11:74eeb8871fe6 467 if(c=='t')
simontruelove 11:74eeb8871fe6 468 {
simontruelove 11:74eeb8871fe6 469 pc.printf("%f C\n\r",temp);
simontruelove 11:74eeb8871fe6 470 }
simontruelove 4:3aedc9246ae4 471 }
simontruelove 5:4e5c644d5cc3 472 }
simontruelove 5:4e5c644d5cc3 473
simontruelove 5:4e5c644d5cc3 474 void RPM (void)
simontruelove 5:4e5c644d5cc3 475 {
simontruelove 10:808cb9052f14 476 wheel.ResetWhoop(); //PulseCount2_==80, whoop_=1;
simontruelove 10:808cb9052f14 477 TimePerClick = (t.read_us()); //read timer in microseconds
simontruelove 10:808cb9052f14 478 t.reset(); //reset timer
simontruelove 10:808cb9052f14 479 TimePerRev = TimePerClick * (800/z); //z = 80 (PulseCount2_==80)
simontruelove 10:808cb9052f14 480 TimePerRev = TimePerRev / 1000; //
simontruelove 5:4e5c644d5cc3 481 RPS = 10000000 / TimePerRev;
simontruelove 5:4e5c644d5cc3 482 rpm = (RPS * 60)/10000;
simontruelove 10:808cb9052f14 483 Aout=((0.30303*rpm)/1000); // for 500 rpm (0.30303*500/1000)*3.3V = 0.500V
simontruelove 11:74eeb8871fe6 484 ReadKType();
simontruelove 10:808cb9052f14 485 //if(rpm < 800)
simontruelove 10:808cb9052f14 486 //{
simontruelove 10:808cb9052f14 487 // AdjCW = 0;
simontruelove 10:808cb9052f14 488 // AdjACW = 3;
simontruelove 10:808cb9052f14 489 //}
simontruelove 10:808cb9052f14 490 //if(rpm > 799 and rpm < 2000)
simontruelove 10:808cb9052f14 491 //{
simontruelove 10:808cb9052f14 492 // AdjCW = 1;
simontruelove 10:808cb9052f14 493 // AdjACW = 2;
simontruelove 10:808cb9052f14 494 //}
simontruelove 10:808cb9052f14 495 //if(rpm >1999)
simontruelove 10:808cb9052f14 496 //{
simontruelove 10:808cb9052f14 497 // AdjCW = 2;
simontruelove 10:808cb9052f14 498 // AdjACW = 1;
simontruelove 10:808cb9052f14 499 //}
simontruelove 10:808cb9052f14 500
simontruelove 10:808cb9052f14 501 //pc.printf("rpm = %i\r", rpm);
simontruelove 6:f7028034aabb 502 //pc.printf("StateA= %i, StateB= %i, StateC= %i, Pulses = %i\n\r", StateA, StateB, StateC, wheel.getPulses());
simontruelove 7:b8de1529c7fc 503 }
simontruelove 7:b8de1529c7fc 504
simontruelove 7:b8de1529c7fc 505 void VelocityLoop (void)
simontruelove 7:b8de1529c7fc 506 {
simontruelove 10:808cb9052f14 507 diff = SetPoint - rpm; //difference between setpoint and the RPM measurement
simontruelove 10:808cb9052f14 508 duty = duty + (diff*0.00001); //duty is adjusted to speed up or slow down until difference = 0
simontruelove 10:808cb9052f14 509 if (duty > 1) //limits for duty. Motor will not operate below 0.96. 1 = max
simontruelove 7:b8de1529c7fc 510 {
simontruelove 9:061600a6c750 511 duty = 1;
simontruelove 7:b8de1529c7fc 512 }
simontruelove 10:808cb9052f14 513 if (duty <0.01) //3A min duty 0.96, 6.5A min duty 0.4
simontruelove 10:808cb9052f14 514 {
simontruelove 10:808cb9052f14 515 duty = 0.01;
simontruelove 7:b8de1529c7fc 516 }
simontruelove 10:808cb9052f14 517 //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 518 }
simontruelove 11:74eeb8871fe6 519
simontruelove 11:74eeb8871fe6 520 void ReadKType(void)
simontruelove 11:74eeb8871fe6 521 {
simontruelove 11:74eeb8871fe6 522 int i = 0;
simontruelove 11:74eeb8871fe6 523 int Readout = 0;
simontruelove 11:74eeb8871fe6 524
simontruelove 11:74eeb8871fe6 525 cs1 = 0;
simontruelove 11:74eeb8871fe6 526 //SerialClock = 0; //set clock to 0
simontruelove 11:74eeb8871fe6 527 //wait_ms(1);
simontruelove 11:74eeb8871fe6 528 SerialClock = 1; //clock once to set to the 13 bit temp data
simontruelove 11:74eeb8871fe6 529 wait_ms(0.000001);
simontruelove 11:74eeb8871fe6 530 SerialClock = 0;
simontruelove 11:74eeb8871fe6 531 wait_ms(0.000001);
simontruelove 11:74eeb8871fe6 532
simontruelove 11:74eeb8871fe6 533 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 534 {
simontruelove 11:74eeb8871fe6 535 if(DOut == 1) //check data, store results in readout
simontruelove 11:74eeb8871fe6 536 {
simontruelove 11:74eeb8871fe6 537 Readout |= (1<<i);
simontruelove 11:74eeb8871fe6 538 }
simontruelove 11:74eeb8871fe6 539 else
simontruelove 11:74eeb8871fe6 540 {
simontruelove 11:74eeb8871fe6 541 Readout |= (0<<i);
simontruelove 11:74eeb8871fe6 542 }
simontruelove 11:74eeb8871fe6 543
simontruelove 11:74eeb8871fe6 544 SerialClock = 1; //clock to the next bit
simontruelove 11:74eeb8871fe6 545 wait_ms(0.000001);
simontruelove 11:74eeb8871fe6 546 SerialClock = 0;
simontruelove 11:74eeb8871fe6 547 wait_ms(0.000001);
simontruelove 11:74eeb8871fe6 548 }
simontruelove 11:74eeb8871fe6 549 temp = Readout * 0.125; //get the real temp value which is a float
simontruelove 11:74eeb8871fe6 550 //pc.printf("%f\n\r",temp);
simontruelove 11:74eeb8871fe6 551 Readout = 0;
simontruelove 11:74eeb8871fe6 552 cs1 = 1;
simontruelove 3:4249dbdf7ed3 553 }