User Tools

Site Tools


pid_control_-_pneumatics_inverted_pendulum

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revisionPrevious revision
pid_control_-_pneumatics_inverted_pendulum [2017/07/10 17:18] – [Matlab Code] hyunheeleepid_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:
    M_errSum = M_errSum + error - 1.0*(U - controlU);    M_errSum = M_errSum + error - 1.0*(U - controlU);
        
-   OnRev(MOTOR, controlU); 
-} 
- 
-sub MotorOpenLoop(float speed) 
-{ 
-   if(speed > FULL_SPEED) 
-   { 
-      controlU = FULL_SPEED; 
-   } 
-   else if(speed < -FULL_SPEED) 
-   { 
-      controlU = -FULL_SPEED; 
-   } 
-   else 
-   { 
-      controlU = speed; 
-   } 
- 
-   OnRev(MOTOR, controlU); 
-} 
- 
-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; 
- 
-   while(true) 
-   { 
-      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's rotation direction. 
-   float Gm_p = -(-7.07106781265702);     //Gain on Motor Position 
-   float Gp_p = -361.604865668371;        //Gain on Pendulum Position 
-   float Gm_v = -(-7.20613568524004);     //Gain on Motor Velocity 
-   float Gp_v = -9.15404034187486;       //Gain on Pendulum velocity 
- 
-   Wait(MS_10); 
-    
-   while(true) 
-   { 
-      if(flag_balanceControl == true) 
-      { 
-         MotorOpenLoop(Gp_p*Prad + Gp_v*d_Prad + Gm_p*Mrad + Gm_v*d_Mrad); 
-      } 
-      Wait(MS_20); 
-   } 
-} 
- 
-task Display() 
-{ 
-   while(true) 
-   { 
-      TextOut(0, LCD_LINE1, FormatNum("Frq: %.1f Hz", 1/dt), DRAW_OPT_CLEAR_LINE); 
-      TextOut(0, LCD_LINE2, FormatNum("P deg: %d", Pdeg), DRAW_OPT_CLEAR_LINE); 
-      TextOut(0, LCD_LINE3, FormatNum("M deg: %d", Mdeg), DRAW_OPT_CLEAR_LINE); 
-      TextOut(0, LCD_LINE4, FormatNum("Time: %.3f", GTime), DRAW_OPT_CLEAR_LINE); 
-      TextOut(0, LCD_LINE5, FormatNum("Flag_B: %d", flag_balanceControl), DRAW_OPT_CLEAR_LINE); 
-      TextOut(0, LCD_LINE6, FormatNum("Ctrl U: %d", controlU), DRAW_OPT_CLEAR_LINE); 
-      Wait(MS_200); 
-   } 
-} 
- 
-task WriteData() 
-{ 
-   short bytesWritten; 
-   string write; 
-   long cnt = 0; 
-    
-   CreateFile("data_sp.txt", 51200, fileHandle); 
-    
-   while(true) 
-   { 
-      ++cnt; 
-      write = NumToStr(cnt); 
-      write = StrCat(write, " ", NumToStr(GTime), " ", NumToStr(Mrad), " ", NumToStr(d_Mrad), " ", NumToStr(Prad), " ", NumToStr(d_Prad)); 
-      WriteLnString(fileHandle, write, bytesWritten); 
-      Wait(MS_5); 
-   } 
-} 
- 
-task main() 
-{ 
-   DeleteFile("data_sp.txt"); 
  
-   prev_tick = CurrentTick(); 
-   Precedes(ReadEncoder, BalanceControl, Display); 
-   //Precedes(ReadEncoder, Display); 
-   //Precedes(ReadEncoder, WriteData, Display); 
-   //Precedes(ReadEncoder, BalanceControl, WriteData, Display); 
-} 
 </code> </code>
  
            
pid_control_-_pneumatics_inverted_pendulum.txt · Last modified: 2017/07/10 17:18 by hyunheelee