algoritmo
DESCRIPTION
networkTRANSCRIPT
\begin{algorithm}
\caption{ Algoritmo do Segway}
\begin{algorithmic}
\item{SETUP}
\STATE $dt \leftarrow 0.03$
\STATE $DtProgramFlow \leftarrow 0$
\begin{itemize}
\item Reset todos os motores
\end{itemize}
\STATE $ph \leftarrow 0$
\STATE $dphDt \leftarrow 0$
\STATE $encoderSum \leftarrow 0$
\STATE $th \leftarrow 0$
\STATE $dthDt \leftarrow 0$
\STATE $AveDthDt \leftarrow 0$
\begin{itemize}
\item Reset Giroscpio (Funo)
\end{itemize}
\STATE $GyroSensor "1" (Measure Rate) \leftarrow Mede "Rate"$
\begin{itemize}
\item Zero Giro Rate
\end{itemize}
\WHILE{$"Gyro Reset Wait" LOOP $}
\STATE $Gyro Sensor "1" Measure angle and Rate \leftarrow Mede "Rate and Angle"$
\STATE $IsNumber \leftarrow "Rate Angle" \leq \hspace{0.5 cm}0 \hspace{0.5 cm} or \hspace{0.5 cm} "Rate Angle" \hspace{0.5 cm}\geq \hspace{0.5 cm} 0$
\STATE $isNumber \leftarrow TRUE$
\ENDWHILE
\WHILE{$"GyroSensor Angle Rate "$}
\STATE $Range Inside \leftarrow Lower - 1 and Upper +1$
\STATE $ Range \leftarrow TRUE$
\STATE $isNumber \leftarrow TRUE$
\ENDWHILE
\STATE $ePrev \leftarrow 0$
\STATE $etd \leftarrow 0$
\STATE $ePrev \leftarrow 0$
\STATE $Kp \leftarrow 1$
\STATE $Ki \leftarrow 0.01$
\STATE $Kd \leftarrow 0$
\end{algorithmic}
\end{algorithm}
\begin{algorithm}
\begin{algorithmic}
\WHILE{$"TRUE" LOOP$}
\item{PID}
\begin{itemize}
\item ReadGiro
\end{itemize}
\STATE $aveDthDt \leftarrow "Read"$
\STATE $GyroSensor "1" \leftarrow Rate Angle$
\STATE $MathAdvanced \leftarrow (0.001*a)+(0.999*b)$
\STATE $aveDthDt \leftarrow "MathAdvanced"$
\STATE $MathSubtract" \leftarrow Rate "GyroSensor" and aveDthDt value$
\STATE $dthDt \leftarrow "MathSubtract"$
\STATE $dt \leftarrow "Read"$
\STATE $th \leftarrow "Read"$
\STATE $MathAdvanced \leftarrow "th" + ("dt"*"MathSubtract")$
\STATE $th \leftarrow "MathAdvanced"$
\STATE $th \leftarrow "Read"$
\STATE $dthDt \leftarrow "Read"$
\STATE $MathMultiply \leftarrow "15" and "th"$
\STATE $MathMultiply \leftarrow "0.8" and "dthDt"$
\ENDWHILE
\begin{itemize}
\item ReadWHELLS
\end{itemize}
\STATE $encoderSUM \leftarrow "Read"$
\begin{itemize}
\item MOTORES - MEASUREdegrees
\end{itemize}
\STATE $MathADD \leftarrow "Motor\hspace{0.5 cm} Rotation\hspace{0.5 cm} A"\hspace{0.5 cm} and "Motor\hspace{0.5 cm} Rotation \hspace{0.5 cm} D"$
\STATE $encoderSUM \leftarrow "MathADD"$
\STATE $MathSubtract \leftarrow "MathADD" and "encoderSUM"$
\STATE $dphDt \leftarrow "READ"$
\STATE $dt \leftarrow "READ"$
\STATE $MathAdvanced \leftarrow [0.75 * "dphDt" + 0.25 * "MathSubtract" * "dt")]$
\STATE $dphDt \leftarrow "MathAdvanced"$
\STATE $ph \leftarrow "READ"$
\STATE $MathADD \leftarrow "ph" and "MathSubtract"$
\STATE $ph \leftarrow "MathADD"$
\STATE $ph \leftarrow "READ"$
\STATE $dphDt \leftarrow "READ"$
\begin{itemize}
\item ERRO
\end{itemize}
\STATE $MathMultiply \leftarrow "0.8" and "Read Wheels"$
\STATE $MathMultiply \leftarrow "0.8" and "Read Wheels"$
\STATE $E \leftarrow ["(th*15) + (dthDT*0.8) + (ph*0.08) + (dphDt*0.008)"]$
\STATE $deDT "1" \leftarrow [("E" - "ePrev") / "dt"]$
\STATE $edt \leftarrow [("E"*"dt") + "edt"]$
\STATE $ePrev \leftarrow "E"$
\STATE $PID \leftarrow [("Rp"*"E") + ("Ki"*"edt") + ("kd" * deDT)]$
\STATE $PID \leftarrow [MAX. (-100 MIN. ("PID,100))]$
\end{algorithmic}
\end{algorithm}