Diff: angle_error.h

Revision:

58:e8426e488925

Child:

59:3e5e5b97ca75

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/angle_error.h	Wed Apr 01 16:08:05 2015 +0000
@@ -0,0 +1,81 @@
+extern int angle_send;
+
+void update_euler(double *z_arr, char*output, int &angle_send)
+{
+    lastpitch = 0.9 * lastpitch + 0.1 * (toDegrees(get_pitch()*3.5));
+    lastroll = 0.9 * lastroll + 0.1 * toDegrees(get_roll()*3.5);
+
+    static double x_;
+    static double y_;
+    static double z_;
+    get_angle(z_,y_,x_);
+    angle_send = 0;
+    arr_pushback(z_/5.0, z_arr, 5);
+    //z_summ = arr_sum((int)5, z_arr);
+    //          z_summ *= 5;
+    //        z_summ = constrains(z_summ,-45.0,45.0);
+
+    sprintf(output,"$EULER,%f,%f,%f\r\n",x_,y_,z_);
+    // bluetooth.puts(output);
+    //pc.puts(output);
+}
+
+double new_bearing(double track_, int avgpos_, double *track_arr_, double *z_arr_)
+{
+    arr_pushback((track_-90)*-1,track_arr_,10);
+    double track_avg_ = arr_avg(track_arr_,abs(avgpos_));
+    //////////////////////////////////////////////////////////////////////////////////////////////////////////////
+    //  double z_summ = z_arr_[0]*abs(avgpos_)*2;//arr_sum((int)5, z_arr_);///////////////////////////////////////
+    //////////////////////////////////////////////////////////////////////////////////////////////////////////////
+    double z_summ = z_arr_[0]*abs(kp);////////////////////////////////////////////////////////////////////////////
+    //////////////////////////////////////////////////////////////////////////////////////////////////////////////
+    z_summ = constrains(z_summ,-45.0,45.0);
+    track_avg_ -= z_summ;
+    // pc.printf("$NEWANGLE,%f,    %f,    %f\r\n",track_avg_, track_arr[0], z_summ);
+
+    return track_avg_;
+}
+
+Point on_linePoint(double lookahead, Point pos, Point start, Point end, double bearing)
+{
+    Point forward;
+
+    double mline = (end.GetY()-start.GetY())/(end.GetX()-start.GetX());
+    double mp3 = - 1 / mline;
+    double b = pos.GetY() - ( mp3 * pos.GetX());
+    double bline = end.GetY()-mline*end.GetX();
+    double x = (b-bline)/(mline-mp3);
+    double y = mline * x + bline;
+
+    double x5;
+    double y5;
+    get_latlon_byangle(x,y,lookahead,bearing,y5,x5);
+  //  pc.printf("bearing,%f  %f  %f  %f  %f \r\n",x,y,x5,y5,bearing);
+    forward.SetX(x5);
+    forward.SetY(y5);
+
+    return forward;
+}
+
+double bearing_to_line(double *z_array, double *track_array,double la,Point pos, Point start, Point end, double lookahead_time,double scl_, double avgp, Point forw)
+{
+    avgp = abs(avgp);
+    //  double gyroDPS = z_array[0]*5;
+    double antenna_avg = arr_avg(track_array,avgp);
+
+    for ( int i = 0; i < avgp / 2; i++ ) {
+        antenna_avg += z_array[i];
+    }
+
+    antenna_avg = angle_normal(antenna_avg, 0, 360);
+    Point forward_pos = on_linePoint(la, pos,start,end,track_array[0]);
+
+   // pc.printf("POS,%f   %f   %f   %f\r\n",forward_pos.GetX(),forward_pos.GetY(),pos.GetX(),pos.GetY());
+
+    double heading_to_line = CalculateHeading(pos.GetX(), pos.GetY(), forward_pos.GetX(), forward_pos.GetY());
+    double error = heading_err(antenna_avg, heading_to_line);
+
+    heading_to_line = angle_normal(heading_to_line,-180,180);
+    //pc.printf("ANGLES,%f    %f    %f \r\n",antenna_avg,heading_to_line,error);
+    return heading_to_line;
+}
\ No newline at end of file