Merged to branch
Dependencies: USBDevice mbed EquatorStrutController LightWeightSerialTransmit
Fork of EquatorStrutDigitalMonitor by
main.cpp
- Committer:
- alpesh
- Date:
- 2014-08-13
- Revision:
- 13:18c376e5dc9a
- Parent:
- 12:814db1249a19
- Child:
- 14:67466da6663d
File content as of revision 13:18c376e5dc9a:
#include "mbed.h" #include "RawSerial.h" DigitalIn RGHSinState(P0_11); DigitalIn RGHCosState(P0_12); DigitalIn HallSensorState(P0_2); InterruptIn RGHSinInterrupt(P0_11); InterruptIn RGHCosInterrupt(P0_12); InterruptIn HallSensor(P0_2); DigitalOut ResetLine(P1_29); PwmOut PhaseA(P0_9); PwmOut PhaseB(P0_8); Timer RunningTime; bool Enabled = false; volatile bool Homing = false; volatile bool HallTriggered = false; RawSerial pc(P1_27, P1_26); volatile char PinState = 0; volatile int direction = 0; volatile double position = 0.0; double currentPower = 0.0; volatile int lastTime = 0; const int arraySize = 50; volatile int intteruptPeriodArray[arraySize]; volatile int arrayTotal = 0; volatile char arrayPos = 0; volatile int interruptPeriod = 0; char counter = 0; void SmoothingAdd(int input) { //arrayTotal -= intteruptPeriodArray[arrayPos]; //arrayTotal += input; intteruptPeriodArray[arrayPos] = input * direction; if (arrayPos == arraySize-1) { arrayPos = 0; } else { arrayPos++; } //interruptPeriod = arrayTotal / 15; } int SmoothedInterruptPeriod() { int arrayTotal = 0; for (char i = 0; i < arraySize; i++) { arrayTotal += intteruptPeriodArray[i]; } return arrayTotal / arraySize; } void RGHSinHandler() { if (PinState == 2) { return; } else if (PinState == 1) { PinState = 0 |(RGHSinState << 1) | RGHCosState; if(PinState == 3) { direction = 1; position += 0.04 * direction; SmoothingAdd(RunningTime.read_us() - lastTime); interruptPeriod = RunningTime.read_us() - lastTime; lastTime = RunningTime.read_us(); } } else { PinState = 0 |(RGHSinState << 1) | RGHCosState; } } void RGHCosHandler() { if (PinState == 1) { return; } else if (PinState == 2) { PinState = 0 |(RGHSinState << 1) | RGHCosState; if (PinState == 3) { direction = -1; position += 0.04 * direction; SmoothingAdd(RunningTime.read_us() - lastTime); interruptPeriod = RunningTime.read_us() - lastTime; lastTime = RunningTime.read_us(); } } else { PinState = 0 |(RGHSinState << 1) | RGHCosState; } } void SetPower(double power) { currentPower = power; if(!Enabled) { return; } if (power > 1.0 || power < -1.0) { return; } PhaseA = (power + 1.0) / 2; PhaseB = 1.0 - ((power + 1.0) / 2); } void Enable() { SetPower(0.0); ResetLine = 1; Enabled = true; } void Disable() { ResetLine = 0; SetPower(0.0); Enabled = false; } void Home() { if (!Enabled) { Enable(); } if (HallSensorState == 1) { Homing = true; HallTriggered = false; SetPower(-0.6); while (!HallTriggered) { wait(0.5); } } SetPower(1.0); while (position < 20.0) { } Homing = true; HallTriggered = false; SetPower(-0.4); while (!HallTriggered) { wait(0.5); } } double GetSpeed() { //if ((RunningTime.read_us() - lastTime) > 1000 || SmoothedInterruptPeriod() == 0) //{ // return 0.0; //} return (0.04)/((double)SmoothedInterruptPeriod() / 1000000.0); //return SmoothedInterruptPeriod(); } double PosError = 0; //This has been defined here as it's being used in the serial transmit function double VelError = 0; void SerialOut(double outputValue) { int outChar = 0; if (outputValue < 0.0) { pc.putc('-'); outputValue *= -1.0; } if (outputValue >= 100.0) { outChar = outputValue / 100; pc.putc(outChar + 48); outputValue -= outChar * 100.0; } if (outputValue >= 10.0) { outChar = outputValue / 10; pc.putc(outChar + 48); outputValue -= outChar * 10.0; } else if(outChar > 0) { pc.putc('0'); } if (outputValue >= 1.0) { outChar = outputValue; pc.putc(outChar + 48); outputValue -= outChar; } else { pc.putc('0'); } if (outputValue >= 0.1) { pc.putc('.'); outChar = outputValue * 10; pc.putc(outChar + 48); outputValue -= (double)outChar / 10.0; } else { pc.putc('.'); pc.putc('0'); } if (outputValue >= 0.01) { outChar = outputValue * 100; pc.putc(outChar + 48); outputValue -= (double)outChar / 100.0; } else { pc.putc('0'); } if (outputValue >= 0.001) { outChar= outputValue * 1000; pc.putc(outChar + 48); } } double PosKpGain = 0.0; double PosKiGain = 0.0; double PosKdGain = 0.0; double VelKpGain = 0.0; double VelKiGain = 0.0; double VelKdGain = 0.0; void SerialTransmit() { SerialOut(RunningTime.read()); pc.putc('\t'); SerialOut(position); pc.putc('\t'); SerialOut(currentPower); pc.putc('\t'); SerialOut(GetSpeed()); pc.putc('\t'); SerialOut(PosError); pc.putc('\t'); SerialOut(PosKpGain); pc.putc('\t'); SerialOut(PosKiGain); pc.putc('\t'); SerialOut(PosKdGain); pc.putc(10); pc.putc(13); } void HallEffectFall() { RGHSinInterrupt.disable_irq(); RGHCosInterrupt.disable_irq(); if (direction < 0) { SetPower(0.0); if (Homing) { HallTriggered = true; Homing = false; position = 0.0; } } RGHSinInterrupt.enable_irq(); RGHCosInterrupt.enable_irq(); } double SetPoint = 50.0; //Target Position in Millimeter per second double PosProError; double PosIntError; double PosDifError; double VelProError; double VelIntError; double VelDifError; int errorcounter; double PosPreviousError [10]; double VelPreviousError [10]; double PwmChange=0; double pwm; double TargetPwm; double TargetVelocity; double PosiState; double VeliState; int PreviousTime = 0; double vMax = 60; void Controller () { /////////////////////////////////////////////////////////////////////////////////////////////// //Position PID /////////////////////////////////////////////////////////////////////////////////////////////// //if (position <= SetPoint || 1) //{ int timeStep = RunningTime.read_us() - PreviousTime; PreviousTime = RunningTime.read_us(); double integral_velmax = vMax/VelKiGain; double integral_velmin = -vMax/VelKiGain ; PosError = SetPoint - position; PosProError = PosError * PosKpGain; PosDifError = (PosError - PosPreviousError[errorcounter]) / timeStep; PosiState += PosError; if (PosiState > integral_velmax) { PosiState = integral_velmax; } else if (PosiState < integral_velmin) { PosiState = integral_velmin; } PosIntError = PosKiGain * PosiState; TargetVelocity = (PosKpGain * PosError + PosKdGain * PosDifError + PosIntError); /////////////////////////////////////////////////////////////////////////////////////////////// //Velocity PID /////////////////////////////////////////////////////////////////////////////////////////////// double integral_pwmmax = 1.0/VelKiGain; double integral_pwmmin = -1.0/VelKiGain; VelError = TargetVelocity - GetSpeed(); VelProError = VelError * VelKpGain; VelDifError = (VelError - VelPreviousError[errorcounter]) / timeStep; //Calculate the integral state with appropriate limiting VeliState += VelError; if (VeliState > integral_pwmmax) { VeliState = integral_pwmmax; } else if (VeliState < integral_pwmmin) { VeliState = integral_pwmmin; } VelIntError = VelKiGain * VeliState; TargetPwm = (VelKpGain * VelError + VelKdGain * VelDifError + VelIntError); if (TargetPwm > 1.0) { pwm = 1.0; } else if (TargetPwm < -1.0) { pwm = -1.0; } else { pwm = TargetPwm; } SetPower(pwm); errorcounter ++; if (errorcounter > 9) { errorcounter = 0; } PosPreviousError[errorcounter] = PosError; VelPreviousError[errorcounter] = VelError; // } } int main() { RGHSinInterrupt.rise(&RGHSinHandler); RGHCosInterrupt.rise(&RGHCosHandler); HallSensor.fall(&HallEffectFall); HallSensor.mode(PullUp); RunningTime.start(); pc.baud(115200); Home(); //Enable(); errorcounter = 0; PosPreviousError[errorcounter]=0; PreviousTime = RunningTime.read_us(); while(Enabled) { //double pow = 0.0; while(PosKpGain < 1.0) { PosKpGain += 0.01; while(PosKiGain < 1.0) { PosKiGain += 0.01; while(PosKdGain < 1.0) { PosKdGain += 0.01; float iterationStart = RunningTime.read(); while(RunningTime.read()-iterationStart < 10.0) { SerialTransmit(); Controller(); } } } Home(); } Disable(); } } /* Change this throttle value range to 1.0 and 0.0 for car speed m_throttlechange = (m_kpGain * m_error + m_kdGain * m_PosDifError + m_PosIntError); double throttle = m_throttle + m_throttlechange; if (new_throttle > 1.0) { m_throttle = 1.0; } else if (new_throttle < 0.0) { m_throttle = 0.0; } else { m_throttle = new_throttle; }*/