From e72cf3ec7f15b644f57cf824139684ab28ddc181 Mon Sep 17 00:00:00 2001
From: Joshua Pfosi <joshpfosi@elbrys.com>
Date: Sun, 11 May 2014 12:33:03 -0400
Subject: [PATCH] Some refactoring and changed waypts to cycle

---
 gen_data    | Bin 9200 -> 9208 bytes
 gui/main.py |   2 +-
 helper.h    |   6 ++---
 navigate.c  |  65 ++++++++++++++++++++++------------------------------
 trst        | Bin 20900 -> 20956 bytes
 5 files changed, 31 insertions(+), 42 deletions(-)

diff --git a/gen_data b/gen_data
index 48b079374c61d00160d75ea1ccad1496fdaef9b8..91cda0c9386b7c0b7444add5ce1f48d0daa0183f 100755
GIT binary patch
delta 334
zcmez1{=<F44`#+6lYcTt1IbX9YJsz+ci$K2+V0-JjVt8T>7&`3AFxEo7^*QaFr)x6
z4448LY0Qia+RO5TL1K(RtOq1Ea6#DVlQR|V8FMGERJ5<J0?P3-3V@V>)%Ah-2(?q7
z{0rO=`GpX5AS1zG4UhzK;QZ}q{Jm)W!>D{GhLflQ442UOH_-U^(fCg%uT)Cl$;vOz
WC`~L#%P-E%oGhrUjw;}wya51tx;C@`

delta 326
zcmez2{=t324`#*>lYcTt1IbX9YJn4dT9P-u85Z>Vb<8fV)C%4FfF(l4P>F$oAq9wG
zz!1ntV`gL!T96+M5@Q5nJs`1x3&M_{oT+Hfm^yi-qJ4c4P>!Eb0Hh48t_{pbsO^FB
zFK|QTXF}A0j0A%vKoZD-^Vg&Cx1#ZPqw<{?4x$P$oI>MYK;vIW<KLaUQYnEWF(<X6
RBtI{GvZ%5;g5#mQ0RV(@F&h8?

diff --git a/gui/main.py b/gui/main.py
index 2f3696a..7c34143 100644
--- a/gui/main.py
+++ b/gui/main.py
@@ -73,7 +73,7 @@
 NAVBAR = 50
 
 # fake baud rate
-BAUD_RATE = 1
+BAUD_RATE = 0.1
 
 def latToY(targetLat):
     """
diff --git a/helper.h b/helper.h
index 787324f..d24bfd6 100644
--- a/helper.h
+++ b/helper.h
@@ -1,6 +1,6 @@
 /*   File: helper.h
  *   By: Joshua Pfosi, Date: Sat May 10
- *   Last Updated: Sat May 10 21:52:18
+ *   Last Updated: Sun May 11 02:15:50
  *
  *   Organized helper functions
  */
@@ -14,10 +14,10 @@ Angle  ang_btwn_angles         (Angle    a1,        Angle    a2);
 Angle  ang_to_wind             (Angle    wind,      Angle    heading);
 Angle  ang_btwn_positions      (Position pos1,      Position pos2);
 double distance_btwn_positions (Position pos1,      Position pos2);
-int is_upwind                  (Angle    wind_ang,  Angle    ang_to_waypt);
+int    is_upwind               (Angle    wind_ang,  Angle    ang_to_waypt);
 double degrees_to_radians      (double   deg);
 double radians_to_degrees      (double   rad);
-Angle standardize              (Angle    a);
+Angle  standardize             (Angle    a);
 FILE *open_or_abort            (char     *filename, char     *mode);
 
 
diff --git a/navigate.c b/navigate.c
index 8724048..6811c91 100644
--- a/navigate.c
+++ b/navigate.c
@@ -1,6 +1,6 @@
 /*   File: high_level.c
  *   By: Joshua Pfosi, Date: Fri Mar 21
- *   Last Updated: Sat May 10 22:00:28
+ *   Last Updated: Sun May 11 02:21:02
  *
  *   Implementation of navigator for algorithm
  *   Takes in input from sensor, parsed by main.c in a loop and decides
@@ -15,16 +15,12 @@
 #include "state_rep.h"
 #include "helper.h"
 
-int skipper(Navigator nav);
-int adjust_heading(Navigator nav, Angle off);
-int adjust_sails(Navigator nav, Angle to_wind);
-void luff(Navigator nav);
-void trim_sail(Angle trim); /* trim between max and min trim */
-int adjust_rudder(int rudder_angle);
-
-/* subroutines */
-int is_upwind(Angle angle_to_wind, Angle ang_to_waypt);
-void update_pid(Rudder_PID_data pid, Angle error);
+static int  skipper(Navigator nav);
+static int  set_course(Navigator nav, Angle ang_to_waypt);
+static int  adjust_heading(Navigator nav, Angle off);
+static int  adjust_sails(Navigator nav, Angle to_wind);
+       void luff(Navigator nav);
+static void update_pid(Rudder_PID_data pid, Angle error);
 
 /* Args: Viable file for sensor input
  * Purpose: Parse input and pass data to skipper
@@ -80,26 +76,24 @@ int read_data(FILE *input) {
 /* Args: Navigator as parsed from file
  * Purpose: Assesses states and calls library functions to guide robotic boat
  * Returns 0 iff encountered no unresolvable problems */
-int skipper(Navigator nav) {
+static int skipper(Navigator nav) {
+    Position pos   = nav->boat->pos,
+             waypt = nav->waypts[nav->current_waypt]; 
+
+    /* update waypoints - if at end, start again */
+    if (distance_btwn_positions(pos, waypt) < WAYPT_RADIUS)
+        nav->current_waypt = (++nav->current_waypt)%nav->num_waypts;
 
-    Position pos        = nav->boat->pos,
-             waypt      = nav->waypts[nav->current_waypt]; 
+    /* return 0 if everything was resolvable */
+    return set_course(nav, ang_btwn_positions(pos, waypt));
+}
+
+static int set_course(Navigator nav, Angle ang_to_waypt) {
     Angle wind_ang      = nav->env->wind_dir,
           heading       = nav->boat->heading,
-          ang_to_waypt  = ang_btwn_positions(pos, waypt),
           angle_to_wind = abs(ang_to_wind(wind_ang, heading)),
           ideal_ang     = ang_to_waypt;
 
-    /* update waypoints */
-    if (distance_btwn_positions(pos, waypt) < WAYPT_RADIUS) {
-        nav->current_waypt++;
-    }
-    if (nav->current_waypt >= nav->num_waypts) {
-        /* luff if at last waypt */
-        luff(nav);
-        return 0;
-    }
-
     /* if the waypt is in the No-Go-Zone then we calculate the closer tack, and
      * set that to our "ideal" heading */
     if (is_upwind(wind_ang, ang_to_waypt)) {
@@ -114,17 +108,13 @@ int skipper(Navigator nav) {
 
     /* calculate error in heading and adjust heading and sails */
     Angle error = ideal_ang - heading;
-    if (adjust_heading(nav, error) || adjust_sails(nav, angle_to_wind))
-        return 1; /* one of the calls couldn't recover */
-
-    /* return 0 if everything was resolvable */
-    return 0;
+    return adjust_heading(nav, error) || adjust_sails(nav, angle_to_wind);
 }
 
 /* Purpose: Assess current heading and waypoint and adjust accordingly 
  * Returns 0 if successful, nonzero otherwise
  */
-int adjust_heading(Navigator nav, Angle error) {
+static int adjust_heading(Navigator nav, Angle error) {
     float PROPORTIONAL_CONST = 0.2; // low level for minor adjustments
     float INTEGRAL_CONST = 0.0;
     Rudder_PID_data pid = nav->boat->PID;
@@ -158,7 +148,7 @@ int adjust_heading(Navigator nav, Angle error) {
  * Returns 0 if successful, nonzero otherwise
  * currently always returns 0
  */
-int adjust_sails(Navigator nav, Angle to_wind) {
+static int adjust_sails(Navigator nav, Angle to_wind) {
     /* 45 < to_wind < 180 */
     /* MIN_TRIM < trim < MAX_TRIM */
     Angle trim = (float)(MAX_TRIM - MIN_TRIM) / (180 - CLOSE_HAULED_ANGLE)
@@ -177,7 +167,7 @@ int adjust_sails(Navigator nav, Angle to_wind) {
 
 /* Purpose: updates the integral part of our pid control
  */
-void update_pid(Rudder_PID_data pid, Angle error) {
+static void update_pid(Rudder_PID_data pid, Angle error) {
     if(pid->pos > ERROR_HISTORY_CAP) {
         pid->integral -= pid->prev_errors[pid->pos];    
     }
@@ -187,9 +177,8 @@ void update_pid(Rudder_PID_data pid, Angle error) {
 }
 
 void luff(Navigator nav) {
-#ifdef DATA_GEN
-    nav->boat->heading = nav->env->wind_dir; /* head upwind */
-    nav->boat->sail_pos = 90; /* sails all out */
-#endif
-    /* add in real luff function later */
+    Angle error = nav->env->wind_dir - nav->boat->heading;
+
+    adjust_heading(nav, error); /* head up wind */
+    adjust_sails(nav, 180);     /* sails all the way out */
 }
diff --git a/trst b/trst
index 157089e2e132413165bc7ec6df486037f59860cc..a03965f2be900490ae2d7ab45d9b13c41aa21ec8 100755
GIT binary patch
delta 3522
zcmZu!3s6+o89wI%f+*ev0hc#pMNxFY2aU{_@XE?1bqQ00BnrK(?t)vD-C=iY(iyrK
zpX&yP<cw{TG0nDS(xz=PWK82DHZIk)Yo={Bq%+zH>8xqexlU5MAx=VDQ~I6zP}t$l
zoX7Y5&-0&iILnKt>BMO|63Y(f5JCwdcQEoV{MwDa6gt6LQ=4m#K_GxLqt!GM@*Kv!
zHbOk9ge<_k9gTlpy>S<Z*;r~Bjj<c4^`@QUd)~Sh*wi=tLj0Z5C+8hyHEESJ%pOa7
zxFXs_NF~%)qM6VV!2GL1GjPM1_tM^9$RatlJ3d)Vh!jezDdEFLDP&DZA=4;PZXo0(
zo_^38S1tk^7@XozJPZSFF;BH1QqZd0<jatTYe!XI+{5D;N@)&jUtGM>qndsU1$dee
z4;5*u9xd$8iq)`)x1F6_ypqc7(&B#lDYN9}7k&!M^QZ?8HUTrF7wVXR>eK8yxs~+y
z>}YPOdC5Wu&b`jv<C~PkRnbY&lsCeDo7+v_U`2WB=-aF<&q5zzgXsI%uk#ugUK6p7
z&Z}JU5%%xAcQbEi!B*TJQKs2={%ZObo6aw$U$NpP4Glf2DBBv9LW5JVXfZ1`l`>_0
zh;mMf+13bXwd=S(DL(ezr1<FXlmhG`=&bYX)RHp#IlH)|wkU5AaviR9Bf2!)F@eg(
zxl^p@Uu>ayMZtZlpmr063o=$C4QFk{T-nFAn41ef(5&MaF7#+pflITq*vsY;AyZ^g
z^LjeX?wG3znl)XC>pC>u7cf_zU=0OJvxa!hLWA+3HKrV4?t)@^f<09*L_cC#g==>F
z5t&Pu%3;MnzP-wMmD;E=FtsUCkSeEDw!KWFHMTKBc}k_jx-x2z7Srn$V<!tcg~A2w
zR^c{!ky(p6ab%w_TDEq5rkao<g#spJ`8|-K6x;>%N|ruXQZw0PQAMCZ3Jy;30>Ugq
z3bmMdt{BTP;s{!&LXtVCZX5S2jm1+X`UfeL4)JxUWnhf+om0@+66pB$nY!c3sdR1c
zZeu0Aq9rOGS%uZ;9uJJEmqIcJH~}BAV07)c6!OQu`d%^5CfI4_!(5(`a8!}g)Wt@X
zZTJHq9lX-eeYrylej*+G42c^Hs)u>Hv9?LEtHs0*8djw_PADtMCRksZdGN6|M7MsX
z{2AHuQXITe-hEjtdyo)4Kn)K47dZ557O=6>DxoQZ-6|axT$yJE%L-|rS1dcE?wYRM
z&eORYRtl*(af~8sMMsiaV)yD8S1h=)lj7<rrDYzUtcq|GURf(@oNbNWD~$3!RPW~@
z#Y1C+HI?Kog`+MTHYD2?Mdht1J&riMn5?+g+17J63s_@GZef=Wx!|07lW{>+E%x)0
zl3X}=G%8k(DQ~K|An45s_G(G~9tgTr!P9pIJUq8VUS0e=;5r*sjwl!IOi#Z?)fA7}
z&WeW%plnF5D_vhtPiyI)9E13{_3Sb#z7av)+q*BHdA|JYA{su^QJt0(=-X#^^^-k*
zkHhYB%3dFO;u#=*H+h-v_4oFYaq1%1sJ*{WCSOtq`HcXbeusU);rhOld?9$;d+h@<
zjP2xWfxjCmBwT@RWCflPr`;iwXQ<ubviEyspIde~`v#m&FL_BI!hZV`9$&yqUZkq^
zpJ)mht%i}`=kfdWOrq4|?3djGvVEs}z(=Np6cS$t??W5VMe3mKaN76EUXRo1AfHpO
z?Dy<<^*hK-D25v$-s#)lFO%Z}Vyl8t1opVSE}zTY?<GGKGO)0{Na$DW)h3Jlv7qJ0
zQytK<Cb!@!(SmR>30=6A&(!WA%o4_H7_LarMB1n@ni8golo;)aP)~%8q$GqPT7!8{
zd(9AKwar`WmZL@vG~-)?NpA`zRWG@>a*p3&3p(SC?gU=OF&k@csp|%sY3&4_%X$Ql
z822=yTaU2D2dlG(@-_A+Iy=M;fE`_;vA@*WQ8os)2PcYqIIFWgj9Odj-UgZ(KL$Kk
zd{U@c|4+9j?0~iU+X=jt+~<6Bd`$-C+%h!ojei`sp__?$O(J-cxhw}Aw~rLFamkWX
zUZT-voh~n7O`9z_ah<Nz>39iya<e77y;RfwrcSrB#?961On59nkS3iqFz0w`!+e5|
z4?MMD%06mdzLrmC)?*v+T=fW-Yo-TST}$=+6VL)d2)og|V(oK4Gku%}p38@6spjKd
zcD1FR{+-R2>hVjURjL;<1a@3{K*$r=JJJKx!sfTu(>m6Q-o}FHx3jCQ^#K>I=OXwv
zPJv&?#xDFo$3{6%=-8;^DIFUH{DqE<QoRl=QWIa7TBfLqu}e!;93X$y31cDu)bY?a
zG=u->IE;cLPqHY5JH#>3<tua?F4yE6blk4%*X!6gCRRfqKi4P;5b`a<ph9!7UB|{*
z-_0=|K@WbV@&tUkd_-FTIi_RdbTb_rHz=&*n4ZuphCRQHh)>=G0mt08`dv-Je@ViB
zOTssk@U0|#I|=`H2Cn51Y3l3*(vz^5g!5T&OIcuf61zGHZ%D%TCt+(6wk6@MNqEO>
zEZ`rE54WtZIEe-P5bZ~3KSp~N4PV0%bVt#Sq0OyXPO3Y+&H-=rcirCIe!Hg^Pk@X0
zZDk8*5vOdcXA?1-Je#<$O<o~;oj!S&+dtsNQ^}t{%dR=yztYw?+xW}2NV<2A%j0nl
ousv;6WiuVg^1j`&^Ks`cKc3;;csQZ+kB2|Vy!Y6$c>9(A1D7~n$p8QV

delta 3491
zcmZ`*3s98T6~1>DaJ9hh@>~#vfK-fz1(gh%MwXBbziAL`h8Ut|-E<eaDOon`GCo>n
zS5Z@DYs=)OnK;#SJ24q&lIg744w|NQv7{k&rb62^W`-F_#-_imR5VQ$OVfVme*m+w
zcji96bG~!#x#vH3?@qkNkH5wT$E=Ag#yDea0yDM5$&WjY{J7YZ(pWnNg#j`f`tC-?
zeuUZ6%$OsEv9Dm=icalWt><Bmh~bnHJ}N#=skhwveBzM@#_yj!^`Dm_?zUTdMRjT=
zPl)!^mI@2pRf4_}-GW{ROl=jqjvc2@R)3Z*Vp+A@-pyuAil$nMX{wW=&S@!Xi8FZ^
zQJ<sm2b`1gQGk7Yw+WgL!hkG3Eux7@15WuW9YZ=?JE-^~54%nB7gkZ7U9_@QF}(r;
zJWZGnELEwk>bCn8Cw16wgXqm($?L^Wvi<ym$jr$rya3Cy$Omoq4$Kf<v~2>^=f%dH
zO8$oE%vol;g=|S@eopSuZc65RM{k-fxdCx3X9s^-{5xk2e_d4NR`GRW2gWAx{oIE1
zAv4ag^?f3)6CdSXT=J(3*qWb8$d|>byw&_waXGJue<)`291e$~vN}hlXy0vEG}+|)
z6p<z4S4ihgv(=e^SQ*+b-88q4-!wmYUY=P(I!ne8@!k9qJ}!>t*Dn1WcF0l-c9#yf
zO(1iV<P;~mD6ZwND9Be7wMCfTcHuPA;S3wra<f=%Yb^X_redAI^!83w`w7<ajdU?!
zD>nQsQ=G7^<(I{TZB;?J%B>`>N9Bfz+b&8A$})DQ!&9_xa=<wzcZimPBJLMm1rdH;
zOcmU>?Jdgaxl&kBqt7cJQABmB1g6chG{EJ4McP`TidAWYN$yg_F^!BIq{g*;#YJ!7
zR>Peo;`fD5@S~z)=~f;QeM^_uWFuJ!NS2~ui<~wOO!CZPT~jYlX>0k)V)2Ki72$w1
z(1%i^9yUqQCYzejrc#QT3OOp-2I#X9!6FS$rbzrfDY^*iG0;tzFRArTUrPCsU{m!v
zOb|FMbKiCnuvx~?HiluxC3$ZuV_{OC0d`WwH7Ddhr_m>0eOtb6qQ$)lBE*RRmeqNJ
zR$uaDU8L5D4_WIlSB;tv(H+&I-#4sWK*^?InjVmf%=>e}DWj9g-vHx1=PYIWsvYaO
zc+SZAi^O+IO3M+(_GW}=ZH{3BXUu#EHKp)k6xQI?0w%WPWEXCCs+c?sD!0Q*5{ok?
z{$5fXhKDCd&6UIQf0bk6JXb5=M>?~H_C}+-mjIvk+O9A^m}4q>xIzxL(@n8DU!my9
z`z!JfpnZ@S=7R+obwymR#d%c4fpq*l(K#s}SCKOP%u(m8)ISmsf%3esKLs&0B@ooc
zZKMmGVHcDKE$O>jCCdWfFzxtF$6%W@@E587S~X7n8GaLvEfzJ!s|=^o#M8xx4Ii0K
zPZbyPv_YkvK2|!E$rGpB?5SDdw&&bFKWpy|c-$eci#^@Ldb-&gJjgEcuHKFgc8$B)
z4en#F8$7Jv80hYD_qafHvkON0@i(#rQfLT<V9lNj_&h9TFtYdx$h>Zki~W$hJwCTT
z=n8eaJl@V8uQ$m4X<&w3?mdA}ko`2w6=%QWMmA=LQEw>F8*-`c-sJ(W-__mYazE4E
z6JozL8d+p5c8sk9@!P&Yz}v(A%0q6y$KB)c?e(%@Fb3<unEo0Vo@aZ5As5?k@OQe{
z2=@FIBJ%k=*~^BY+t(FjKSrca#2+O7MhuhnWaM>sxq<<&*Tc>kg09}cE}!4ShTu5X
z0QEDWU49q4WWZ?@#TRfJ=nncqzHWbzT{pP>on1KLjM({5Rr@KUdSE(xkD3Pd4m?da
zVYe{}Lu?bRl;KgVrdyHCX+E7AH^hxGK5d*#aU^&k!KV!oUX9hx*6Ik#M@&8R`1&$r
zpMDqhMsH<@k+ZB=_v0j^GONO%KjAIF%L$7;jZM5woNUalxEnu?SB-2BX#BXCX|!8o
zc)dybq9%=r^$**vWBID|RZTi3_COlJ6(H`LnlvKLKHRkaEYQMuM}Y6Pe`%p={SVE$
zRIGQ}!xlWU<a0)o>X>B9(RWfYq7mMVVIih01eU2f1J;xU9b>83J1|otO!f`}XoO*(
z&ElE^6C~syzD(S4R%He7bP&g;i325K!y{ESlbZOenpnp~rErJPsS&2_hQyJ^6*X(H
zT9~2@z;{bgtW>phi<%}o9zJ)Iy=HHcZU6tVwV-aGVsDD!rh0x(aH*dEQ8Y;PhAUj`
zmo^%1aB*JR$d?Gdsh;PG28=7kK8*K^;Z5~nClZ|r2YNwl0XB2`!B_oy6-@b2aj%B;
zMD=SpQKsr&(y$)a2@OZeRsAnC97Ec$ZJ52U38po|Cpw{2HMp+fR@H#r(Xjr-anBI&
z{Jluia4UYtC^5Mj)~mRjFy6`l{*ZF)&+HmuuuL^*)UaN-n>DOoj%^y&OV*?7wE&;j
zuwEm58rCmj6c~X~Y;=Ccjw}=aA5X$ROTuT9@UN5bND{t~gvS?P+Mj*6KtT9%68<y^
zPbJ}hCE?j5oRYHO#PFXnkMSI%<%*ieN@~mJ#fo%Q5{^K60R15PA@mo}zbD2YTZ=Ev
z9Q!crNAE|Epx=#Yx0W({ThQARw7a{!yF=anPO-1K#54!qYF<4Lt~9&mLCa=WzVcdD
zee+8$KG@taPb_Omh<jRd#kH1V;c3YgkG77KXpbAcgL97?-Xi6R#v3baTSoo|<a;xf