pid_control_-_pneumatics_inverted_pendulum
Differences
This shows you the differences between two versions of the page.
Both sides previous revisionPrevious revision | |||
pid_control_-_pneumatics_inverted_pendulum [2017/07/10 17:18] – [Matlab Code] hyunheelee | pid_control_-_pneumatics_inverted_pendulum [2017/07/10 17:18] (current) – [Arduino code for PID] hyunheelee | ||
---|---|---|---|
Line 141: | Line 141: | ||
===== Arduino code for PID ===== | ===== Arduino code for PID ===== | ||
<code java> | <code java> | ||
- | #define MOTOR OUT_A | ||
- | #define PNDLM OUT_C | ||
- | #define FULL_SPEED 100 | ||
- | #define D2R 0.0175 // converts degree to radian | ||
- | #define R2D 57.2958 // converts radian to degree | ||
- | |||
- | long prev_tick = 0; | ||
- | long prev_Mdeg = 0; | ||
- | long prev_Pdeg = 0; | ||
- | long Mdeg = 0; | ||
- | long Pdeg = 0; | ||
- | long M_diff = 0; | ||
- | long P_diff = 0; | ||
- | |||
- | float dt = 0.0; | ||
- | float GTime = 0.0; | ||
- | float Mrad = 0.0; | ||
- | float Prad = 0.0; | ||
- | float d_Mrad = 0.0; | ||
- | float d_Prad = 0.0; | ||
- | float d_Mdeg = 0.0; | ||
- | float d_Pdeg = 0.0; | ||
- | float M_errSum = 0.0; | ||
char controlU = 0; | char controlU = 0; | ||
Line 191: | Line 168: | ||
| | ||
- | | ||
- | } | ||
- | |||
- | sub MotorOpenLoop(float speed) | ||
- | { | ||
- | | ||
- | { | ||
- | controlU = FULL_SPEED; | ||
- | } | ||
- | else if(speed < -FULL_SPEED) | ||
- | { | ||
- | controlU = -FULL_SPEED; | ||
- | } | ||
- | else | ||
- | { | ||
- | controlU = speed; | ||
- | } | ||
- | |||
- | | ||
- | } | ||
- | |||
- | task ReadEncoder() | ||
- | { | ||
- | const int w = 4; | ||
- | float M_mov[w] = {0.0,}; | ||
- | float P_mov[w] = {0.0,}; | ||
- | float sum_Mmov = 0.0; | ||
- | float sum_Pmov = 0.0; | ||
- | int cnt = 0; | ||
- | |||
- | | ||
- | { | ||
- | dt = (CurrentTick() - prev_tick)*0.001; | ||
- | Mdeg = MotorRotationCount(MOTOR); | ||
- | Pdeg = MotorRotationCount(PNDLM); | ||
- | M_diff = Mdeg - prev_Mdeg; | ||
- | P_diff = Pdeg - prev_Pdeg; | ||
- | prev_Mdeg = Mdeg; | ||
- | prev_Pdeg = Pdeg; | ||
- | |||
- | d_Mdeg = M_diff/dt; | ||
- | d_Pdeg = P_diff/dt; | ||
- | |||
- | Mrad = Mdeg*D2R; | ||
- | Prad = Pdeg*D2R; | ||
- | |||
- | d_Mrad = d_Mdeg*D2R; | ||
- | d_Prad = d_Pdeg*D2R; | ||
- | |||
- | prev_tick = CurrentTick(); | ||
- | GTime += dt; | ||
- | |||
- | Wait(MS_20); | ||
- | } | ||
- | } | ||
- | |||
- | task BalanceControl() | ||
- | { | ||
- | // A physical motor rotation direction is reversed as a model' | ||
- | float Gm_p = -(-7.07106781265702); | ||
- | float Gp_p = -361.604865668371; | ||
- | float Gm_v = -(-7.20613568524004); | ||
- | float Gp_v = -9.15404034187486; | ||
- | |||
- | | ||
- | |||
- | | ||
- | { | ||
- | if(flag_balanceControl == true) | ||
- | { | ||
- | | ||
- | } | ||
- | Wait(MS_20); | ||
- | } | ||
- | } | ||
- | |||
- | task Display() | ||
- | { | ||
- | | ||
- | { | ||
- | TextOut(0, LCD_LINE1, FormatNum(" | ||
- | TextOut(0, LCD_LINE2, FormatNum(" | ||
- | TextOut(0, LCD_LINE3, FormatNum(" | ||
- | TextOut(0, LCD_LINE4, FormatNum(" | ||
- | TextOut(0, LCD_LINE5, FormatNum(" | ||
- | TextOut(0, LCD_LINE6, FormatNum(" | ||
- | Wait(MS_200); | ||
- | } | ||
- | } | ||
- | |||
- | task WriteData() | ||
- | { | ||
- | short bytesWritten; | ||
- | | ||
- | long cnt = 0; | ||
- | |||
- | | ||
- | |||
- | | ||
- | { | ||
- | ++cnt; | ||
- | write = NumToStr(cnt); | ||
- | write = StrCat(write, | ||
- | WriteLnString(fileHandle, | ||
- | Wait(MS_5); | ||
- | } | ||
- | } | ||
- | |||
- | task main() | ||
- | { | ||
- | | ||
- | | ||
- | | ||
- | // | ||
- | // | ||
- | // | ||
- | } | ||
</ | </ | ||
pid_control_-_pneumatics_inverted_pendulum.txt · Last modified: 2017/07/10 17:18 by hyunheelee