Stewart Coulden-Smith / EquatorStrutController

Equator Strut Controller

Dependents:   EquatorStrutDigitalMonitor

EquatorStrutController.cpp@1:580fded7b5b2, 2014-07-29 (annotated)

Committer:
pyrostew
Date:
Tue Jul 29 11:12:10 2014 +0000
Revision:
1:580fded7b5b2
Parent:
0:a6ade00ff41d
Child:
2:3976e3f43470
Added code to enable current speed to be read.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pyrostew 0:a6ade00ff41d 1 #include "EquatorStrutController.h"
pyrostew 0:a6ade00ff41d 2
pyrostew 0:a6ade00ff41d 3 EquatorStrut::EquatorStrut()
pyrostew 0:a6ade00ff41d 4 {
pyrostew 0:a6ade00ff41d 5 PinState = 0;
pyrostew 0:a6ade00ff41d 6 FullWavePeriod = 0;
pyrostew 0:a6ade00ff41d 7 PartWavePeriod = 0;
pyrostew 0:a6ade00ff41d 8 position = 0.0;
pyrostew 0:a6ade00ff41d 9 direction = 0;
pyrostew 0:a6ade00ff41d 10 Homing = false;
pyrostew 0:a6ade00ff41d 11 HallTriggered = false;
pyrostew 0:a6ade00ff41d 12 Enabled = true;
pyrostew 0:a6ade00ff41d 13
pyrostew 0:a6ade00ff41d 14 ResetLine = new DigitalOut(P1_29);
pyrostew 0:a6ade00ff41d 15 PulseOut1 = new DigitalOut(P1_27);
pyrostew 0:a6ade00ff41d 16 PulseOut2 = new DigitalOut(P1_26);
pyrostew 0:a6ade00ff41d 17
pyrostew 0:a6ade00ff41d 18 Disable();
pyrostew 0:a6ade00ff41d 19
pyrostew 0:a6ade00ff41d 20 RGHSin = new DigitalIn(P0_11);
pyrostew 0:a6ade00ff41d 21 RGHCos = new DigitalIn(P0_12);
pyrostew 0:a6ade00ff41d 22 HallSensor = new InterruptIn(P0_2);
pyrostew 0:a6ade00ff41d 23
pyrostew 0:a6ade00ff41d 24 InputReadTick.attach_us(this, &EquatorStrut::InputRead, 20);
pyrostew 0:a6ade00ff41d 25 (*HallSensor).fall(this, &EquatorStrut::HallEffectFall);
pyrostew 0:a6ade00ff41d 26 (*HallSensor).mode(PullUp);
pyrostew 0:a6ade00ff41d 27
pyrostew 0:a6ade00ff41d 28 PhaseA = new PwmOut(P0_9);
pyrostew 0:a6ade00ff41d 29 PhaseB = new PwmOut(P0_8);
pyrostew 0:a6ade00ff41d 30
pyrostew 0:a6ade00ff41d 31 SinInterruptInterval.start();
pyrostew 0:a6ade00ff41d 32 }
pyrostew 0:a6ade00ff41d 33
pyrostew 0:a6ade00ff41d 34 void EquatorStrut::SetPower(double power)
pyrostew 0:a6ade00ff41d 35 {
pyrostew 0:a6ade00ff41d 36 if(!Enabled)
pyrostew 0:a6ade00ff41d 37 {
pyrostew 0:a6ade00ff41d 38 return;
pyrostew 0:a6ade00ff41d 39 }
pyrostew 0:a6ade00ff41d 40
pyrostew 0:a6ade00ff41d 41 if (power > 1.0 || power < -1.0)
pyrostew 0:a6ade00ff41d 42 {
pyrostew 0:a6ade00ff41d 43 return;
pyrostew 0:a6ade00ff41d 44 }
pyrostew 0:a6ade00ff41d 45
pyrostew 0:a6ade00ff41d 46 *PhaseA = (power + 1.0) / 2;
pyrostew 0:a6ade00ff41d 47 *PhaseB = 1.0 - ((power + 1.0) / 2);
pyrostew 0:a6ade00ff41d 48 }
pyrostew 0:a6ade00ff41d 49
pyrostew 0:a6ade00ff41d 50 double EquatorStrut::GetPosition()
pyrostew 0:a6ade00ff41d 51 {
pyrostew 0:a6ade00ff41d 52 return position;
pyrostew 0:a6ade00ff41d 53 }
pyrostew 0:a6ade00ff41d 54
pyrostew 0:a6ade00ff41d 55 void EquatorStrut::Home()
pyrostew 0:a6ade00ff41d 56 {
pyrostew 0:a6ade00ff41d 57 if (!Enabled)
pyrostew 0:a6ade00ff41d 58 {
pyrostew 0:a6ade00ff41d 59 Enable();
pyrostew 0:a6ade00ff41d 60 }
pyrostew 0:a6ade00ff41d 61
pyrostew 0:a6ade00ff41d 62 Homing = true;
pyrostew 0:a6ade00ff41d 63
pyrostew 0:a6ade00ff41d 64 SetPower(-1.0);
pyrostew 0:a6ade00ff41d 65
pyrostew 0:a6ade00ff41d 66 while (!HallTriggered)
pyrostew 0:a6ade00ff41d 67 {
pyrostew 0:a6ade00ff41d 68 wait(0.5);
pyrostew 0:a6ade00ff41d 69 }
pyrostew 0:a6ade00ff41d 70
pyrostew 0:a6ade00ff41d 71 SetPower(1.0);
pyrostew 0:a6ade00ff41d 72
pyrostew 0:a6ade00ff41d 73 while (position < 20.0)
pyrostew 0:a6ade00ff41d 74 {
pyrostew 0:a6ade00ff41d 75
pyrostew 0:a6ade00ff41d 76 }
pyrostew 0:a6ade00ff41d 77
pyrostew 0:a6ade00ff41d 78 Homing = true;
pyrostew 0:a6ade00ff41d 79
pyrostew 0:a6ade00ff41d 80 SetPower(-0.5);
pyrostew 0:a6ade00ff41d 81
pyrostew 0:a6ade00ff41d 82 while (!HallTriggered)
pyrostew 0:a6ade00ff41d 83 {
pyrostew 0:a6ade00ff41d 84 wait(0.5);
pyrostew 0:a6ade00ff41d 85 }
pyrostew 0:a6ade00ff41d 86 }
pyrostew 0:a6ade00ff41d 87
pyrostew 0:a6ade00ff41d 88 void EquatorStrut::Enable()
pyrostew 0:a6ade00ff41d 89 {
pyrostew 0:a6ade00ff41d 90 SetPower(0.0);
pyrostew 0:a6ade00ff41d 91
pyrostew 0:a6ade00ff41d 92 (*ResetLine) = 1;
pyrostew 0:a6ade00ff41d 93
pyrostew 0:a6ade00ff41d 94 Enabled = true;
pyrostew 0:a6ade00ff41d 95 }
pyrostew 0:a6ade00ff41d 96
pyrostew 0:a6ade00ff41d 97 void EquatorStrut::Disable()
pyrostew 0:a6ade00ff41d 98 {
pyrostew 0:a6ade00ff41d 99 (*ResetLine) = 0;
pyrostew 0:a6ade00ff41d 100
pyrostew 0:a6ade00ff41d 101 SetPower(0.0);
pyrostew 0:a6ade00ff41d 102
pyrostew 0:a6ade00ff41d 103 Enabled = false;
pyrostew 0:a6ade00ff41d 104 }
pyrostew 0:a6ade00ff41d 105
pyrostew 0:a6ade00ff41d 106 double EquatorStrut::CurrentSpeed()
pyrostew 0:a6ade00ff41d 107 {
pyrostew 0:a6ade00ff41d 108 if (SinInterruptInterval.read_us() < 100000)
pyrostew 0:a6ade00ff41d 109 {
pyrostew 0:a6ade00ff41d 110 if (FullWavePeriod > 100000)
pyrostew 0:a6ade00ff41d 111 {
pyrostew 0:a6ade00ff41d 112 return 0.0;
pyrostew 0:a6ade00ff41d 113 }
pyrostew 0:a6ade00ff41d 114 else
pyrostew 0:a6ade00ff41d 115 {
pyrostew 1:580fded7b5b2 116 return (0.04 / ((double)FullWavePeriod / 1000000)) * direction;
pyrostew 0:a6ade00ff41d 117 }
pyrostew 0:a6ade00ff41d 118 }
pyrostew 0:a6ade00ff41d 119 else
pyrostew 0:a6ade00ff41d 120 {
pyrostew 0:a6ade00ff41d 121 return 0.0;
pyrostew 0:a6ade00ff41d 122 }
pyrostew 0:a6ade00ff41d 123 }
pyrostew 0:a6ade00ff41d 124
pyrostew 0:a6ade00ff41d 125 void EquatorStrut::InputRead()
pyrostew 0:a6ade00ff41d 126 {
pyrostew 0:a6ade00ff41d 127 if (PinState == 3)
pyrostew 0:a6ade00ff41d 128 {
pyrostew 0:a6ade00ff41d 129 PinState = 0 | ((*RGHSin) << 1) | (*RGHCos);
pyrostew 0:a6ade00ff41d 130
pyrostew 0:a6ade00ff41d 131 if (PinState == 1)
pyrostew 0:a6ade00ff41d 132 {
pyrostew 0:a6ade00ff41d 133 direction = 1;
pyrostew 0:a6ade00ff41d 134 position += (0.04 * direction);
pyrostew 1:580fded7b5b2 135 FullWavePeriod = SinInterruptInterval.read_us();
pyrostew 1:580fded7b5b2 136 SinInterruptInterval.reset();
pyrostew 0:a6ade00ff41d 137 }
pyrostew 0:a6ade00ff41d 138 else if (PinState == 2)
pyrostew 0:a6ade00ff41d 139 {
pyrostew 0:a6ade00ff41d 140 direction = -1;
pyrostew 0:a6ade00ff41d 141 position += (0.04 * direction);
pyrostew 1:580fded7b5b2 142 FullWavePeriod = SinInterruptInterval.read_us();
pyrostew 1:580fded7b5b2 143 SinInterruptInterval.reset();
pyrostew 0:a6ade00ff41d 144 }
pyrostew 0:a6ade00ff41d 145 }
pyrostew 0:a6ade00ff41d 146 else
pyrostew 0:a6ade00ff41d 147 {
pyrostew 0:a6ade00ff41d 148 PinState = 0 | ((*RGHSin) << 1) | (*RGHCos);
pyrostew 0:a6ade00ff41d 149 }
pyrostew 0:a6ade00ff41d 150 }
pyrostew 0:a6ade00ff41d 151
pyrostew 0:a6ade00ff41d 152 void EquatorStrut::HallEffectFall()
pyrostew 0:a6ade00ff41d 153 {
pyrostew 0:a6ade00ff41d 154 if (direction < 0)
pyrostew 0:a6ade00ff41d 155 {
pyrostew 0:a6ade00ff41d 156 SetPower(0.0);
pyrostew 0:a6ade00ff41d 157
pyrostew 0:a6ade00ff41d 158 if (Homing)
pyrostew 0:a6ade00ff41d 159 {
pyrostew 0:a6ade00ff41d 160 HallTriggered = true;
pyrostew 0:a6ade00ff41d 161 Homing = false;
pyrostew 0:a6ade00ff41d 162 position = 0.0;
pyrostew 0:a6ade00ff41d 163 }
pyrostew 0:a6ade00ff41d 164 }
pyrostew 0:a6ade00ff41d 165 }