Agra-GPS / FreePilot_V2-3

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   FreePilot PinDetect mbed-src

Fork of FreePilot_V2-2 by Agra-GPS

main.cpp@48:5d9c63364c94, 2015-03-14 (annotated)

Committer:
maximbolduc
Date:
Sat Mar 14 01:56:28 2015 +0000
Revision:
48:5d9c63364c94
Parent:
47:d3123bb4f673
Child:
49:c43fa54dd990
splitted

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maximbolduc 26:dc00998140af 1 #include "mbed.h"
maximbolduc 26:dc00998140af 2 #include "PinDetect.h"
maximbolduc 26:dc00998140af 3 #include "Point.h"
maximbolduc 26:dc00998140af 4 #include <vector>
maximbolduc 26:dc00998140af 5 #include "Line.h"
maximbolduc 26:dc00998140af 6 #include "stringUtils.h"
jhedmonton 28:5905886c76ee 7 #include "base.h"
jhedmonton 28:5905886c76ee 8 #include "Config.h"
maximbolduc 30:3afafa1ef16b 9 #include "imu_functions.h"
maximbolduc 32:c57bc701d65c 10 #include "atoh.h"
maximbolduc 47:d3123bb4f673 11 #include "string_utilities.h"
maximbolduc 47:d3123bb4f673 12
maximbolduc 34:c2bc9f9be7ff 13 #include "checksum.h"
maximbolduc 34:c2bc9f9be7ff 14 #include <string.h>
maximbolduc 47:d3123bb4f673 15 #include "gps.h"
maximbolduc 47:d3123bb4f673 16 #include "autosteer.h"
maximbolduc 35:f9caeb8ca31e 17
maximbolduc 30:3afafa1ef16b 18 char *version="FreePilot V2.11 Jtan 20, 2015\r\n";
jhedmonton 27:9ac59b261d87 19 long lastsend_version=0;
jhedmonton 27:9ac59b261d87 20 Timer vTimer; //this timer is int based! Max is 30 minutes
maximbolduc 35:f9caeb8ca31e 21
maximbolduc 26:dc00998140af 22 int checksumm;
maximbolduc 26:dc00998140af 23 double distance_from_line;
maximbolduc 26:dc00998140af 24 double cm_per_deg_lon;
maximbolduc 26:dc00998140af 25 double cm_per_deg_lat;
maximbolduc 26:dc00998140af 26 //all timing objects
maximbolduc 26:dc00998140af 27 Timer gps_connecting;
maximbolduc 26:dc00998140af 28 Timer autosteer_time;
maximbolduc 36:8e84b5ade03e 29 Timer autosteer_timeout; //timeout work near as timer, but they don't give timing. they just trigger an interrupt once the time we assigned it is passed.
maximbolduc 26:dc00998140af 30 Ticker accelerometerTicker;
maximbolduc 26:dc00998140af 31 Ticker gyroscopeTicker;
maximbolduc 26:dc00998140af 32 Ticker filterTicker;
maximbolduc 26:dc00998140af 33 Ticker angle_print;
maximbolduc 35:f9caeb8ca31e 34
jhedmonton 27:9ac59b261d87 35 //Motor
jhedmonton 27:9ac59b261d87 36 PinDetect motor_switch(p16); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
jhedmonton 27:9ac59b261d87 37 DigitalOut enable_motor(p7);
maximbolduc 35:f9caeb8ca31e 38
maximbolduc 37:ac60a8a0ae8a 39 PwmOut pwm1(p21);
maximbolduc 37:ac60a8a0ae8a 40 PwmOut pwm2(p22);
maximbolduc 35:f9caeb8ca31e 41
jhedmonton 27:9ac59b261d87 42 //equipment switches
jhedmonton 27:9ac59b261d87 43 PinDetect boom1(p20,PullUp); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
jhedmonton 27:9ac59b261d87 44 PinDetect boom2(p19); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
jhedmonton 27:9ac59b261d87 45 PinDetect boom3(p18); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
jhedmonton 27:9ac59b261d87 46 PinDetect boom4(p17); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
maximbolduc 35:f9caeb8ca31e 47
jhedmonton 27:9ac59b261d87 48 char boom18; //1 byte
jhedmonton 27:9ac59b261d87 49 char lastboom18; //1 byte
maximbolduc 35:f9caeb8ca31e 50 char boomstate[8]= {'$','F','B','S',0,13,10,0 };
maximbolduc 35:f9caeb8ca31e 51
maximbolduc 35:f9caeb8ca31e 52 double filterg = 100;
maximbolduc 48:5d9c63364c94 53
maximbolduc 26:dc00998140af 54 Point looked_ahead;
maximbolduc 26:dc00998140af 55 Point line_start;
maximbolduc 26:dc00998140af 56 Point line_end;
maximbolduc 26:dc00998140af 57 Point tilt_compensated_position;
maximbolduc 26:dc00998140af 58 Point yaw_compensated_position;
maximbolduc 35:f9caeb8ca31e 59
maximbolduc 32:c57bc701d65c 60 extern int gyro_pos;
maximbolduc 26:dc00998140af 61 double distance_to_line;
maximbolduc 35:f9caeb8ca31e 62
maximbolduc 26:dc00998140af 63 //FreePilot variables
maximbolduc 26:dc00998140af 64 int timer_enabled;
jhedmonton 28:5905886c76ee 65 double motorspeed;
maximbolduc 26:dc00998140af 66 int enable_time;
maximbolduc 26:dc00998140af 67 char* motor_enable_state = 0;
jhedmonton 28:5905886c76ee 68 int motor_enable = 0;
jhedmonton 28:5905886c76ee 69 int lastmotor_enable = 1;
maximbolduc 26:dc00998140af 70 double pwm1_speed;
maximbolduc 26:dc00998140af 71 double pwm2_speed;
jhedmonton 28:5905886c76ee 72 long lastsend_motorstate=0;
jhedmonton 28:5905886c76ee 73 Timer motTimer; //this timer is int based! Max is 30 minutes
jhedmonton 28:5905886c76ee 74 Timer btTimer; //measure time for Bluetooth communication
jhedmonton 28:5905886c76ee 75 long lastgetBT=0;
maximbolduc 35:f9caeb8ca31e 76
maximbolduc 26:dc00998140af 77 int msg2_changed = 1;
maximbolduc 26:dc00998140af 78 char* buffer;
maximbolduc 26:dc00998140af 79 double meter_lat = 0;
maximbolduc 26:dc00998140af 80 double meter_lon = 0;
maximbolduc 35:f9caeb8ca31e 81
maximbolduc 26:dc00998140af 82 char msg[256]; //GPS line buffer
maximbolduc 26:dc00998140af 83 char msg2[256];//PC line buffer
maximbolduc 45:ecd8c2e27948 84 char* result;
maximbolduc 26:dc00998140af 85 int printing;
maximbolduc 26:dc00998140af 86 float longitude;
maximbolduc 26:dc00998140af 87 float latitude;
maximbolduc 26:dc00998140af 88 char ns, ew;
maximbolduc 26:dc00998140af 89 int lock;
maximbolduc 26:dc00998140af 90 int flag_gga;
maximbolduc 26:dc00998140af 91 int reading;
maximbolduc 48:5d9c63364c94 92
maximbolduc 26:dc00998140af 93 int connect_time = 10000; //variable to change the time that the serial output all the strings in order to verify if the command was right.
maximbolduc 26:dc00998140af 94 int connecting = 0; //are we still in phase of connecting? based on the connect_time value.
maximbolduc 35:f9caeb8ca31e 95
maximbolduc 26:dc00998140af 96 int angle_send = 0;
maximbolduc 26:dc00998140af 97 int correct_rmc = 1;
maximbolduc 26:dc00998140af 98 double m_lat = 0;
maximbolduc 26:dc00998140af 99 double m_lon = 0;
maximbolduc 26:dc00998140af 100 char* degminsec;
maximbolduc 26:dc00998140af 101 double m_per_deg_lon;
maximbolduc 26:dc00998140af 102 double m_per_deg_lat;
maximbolduc 26:dc00998140af 103 double look_ahead_lon;
maximbolduc 26:dc00998140af 104 double look_ahead_lat;
maximbolduc 26:dc00998140af 105 int active_AB = 0;
maximbolduc 26:dc00998140af 106 double compensation_vector;
maximbolduc 26:dc00998140af 107 char output[256];
maximbolduc 35:f9caeb8ca31e 108
maximbolduc 26:dc00998140af 109 double yaw;
maximbolduc 26:dc00998140af 110 double pitch;
maximbolduc 26:dc00998140af 111 double roll;
maximbolduc 30:3afafa1ef16b 112 double a_x;
maximbolduc 30:3afafa1ef16b 113 double a_y;
maximbolduc 30:3afafa1ef16b 114 double a_z;
maximbolduc 30:3afafa1ef16b 115 double w_x;
maximbolduc 30:3afafa1ef16b 116 double w_y;
maximbolduc 30:3afafa1ef16b 117 double w_z;
maximbolduc 35:f9caeb8ca31e 118
maximbolduc 26:dc00998140af 119 int readings[3];
maximbolduc 26:dc00998140af 120 double Freepilot_bearing;
maximbolduc 36:8e84b5ade03e 121 int time_till_stop = 200;
maximbolduc 26:dc00998140af 122 volatile bool newline_detected = false;
maximbolduc 35:f9caeb8ca31e 123
maximbolduc 35:f9caeb8ca31e 124 void autosteer_done()
maximbolduc 35:f9caeb8ca31e 125 {
maximbolduc 35:f9caeb8ca31e 126 enable_motor = 0;
maximbolduc 35:f9caeb8ca31e 127 }
maximbolduc 35:f9caeb8ca31e 128
maximbolduc 26:dc00998140af 129 Point compensation;
maximbolduc 35:f9caeb8ca31e 130
maximbolduc 26:dc00998140af 131 void yaw_compensate()
maximbolduc 26:dc00998140af 132 {
maximbolduc 33:3e71c418e90d 133 yaw = get_yaw();
maximbolduc 26:dc00998140af 134 }
maximbolduc 34:c2bc9f9be7ff 135
maximbolduc 35:f9caeb8ca31e 136 void pitch_and_roll(double real_bearing)
maximbolduc 34:c2bc9f9be7ff 137 {
maximbolduc 34:c2bc9f9be7ff 138 pitch = get_pitch();
maximbolduc 34:c2bc9f9be7ff 139 roll = get_roll();
maximbolduc 34:c2bc9f9be7ff 140 compensation.SetX(antennaheight * tan(roll) * sin(real_bearing/ 57.295779513)-antennaheight * tan(pitch) * cos(real_bearing/ 57.295779513));
maximbolduc 34:c2bc9f9be7ff 141 compensation.SetY(antennaheight * tan(roll) * cos(real_bearing/ 57.295779513)-antennaheight * tan(pitch) * sin(real_bearing/ 57.295779513));
maximbolduc 34:c2bc9f9be7ff 142 }
maximbolduc 33:3e71c418e90d 143
maximbolduc 26:dc00998140af 144 void process_GPSHEIGHT(char* height_string)
maximbolduc 26:dc00998140af 145 {
maximbolduc 26:dc00998140af 146 char *token;
maximbolduc 26:dc00998140af 147 int token_counter = 0;
maximbolduc 26:dc00998140af 148 char *height = (char *)NULL;
maximbolduc 26:dc00998140af 149 token = strtok(height_string, ",");
maximbolduc 35:f9caeb8ca31e 150 while (token) {
maximbolduc 35:f9caeb8ca31e 151 switch (token_counter) {
maximbolduc 26:dc00998140af 152 case 1:
maximbolduc 26:dc00998140af 153 height = token;
maximbolduc 26:dc00998140af 154 break;
maximbolduc 26:dc00998140af 155 }
maximbolduc 26:dc00998140af 156 token = strtok((char *)NULL, ",");
maximbolduc 26:dc00998140af 157 token_counter++;
maximbolduc 26:dc00998140af 158 }
maximbolduc 35:f9caeb8ca31e 159 if ( height ) {
jhedmonton 28:5905886c76ee 160 antennaheight = atof(height);
maximbolduc 42:854d8cc26bbb 161 // Config_Save();
maximbolduc 26:dc00998140af 162 }
maximbolduc 26:dc00998140af 163 }
maximbolduc 26:dc00998140af 164
maximbolduc 35:f9caeb8ca31e 165 //sets pwm1 and pwm2 and enable_motor
maximbolduc 46:d7d6dc429153 166 void process_ASTEER(char* asteer,bool from_pc)
maximbolduc 46:d7d6dc429153 167 {
maximbolduc 47:d3123bb4f673 168 if ( freepilot_v2 && !from_pc || from_pc && !freepilot_v2) {
maximbolduc 47:d3123bb4f673 169 char *token;
maximbolduc 47:d3123bb4f673 170 int token_counter = 0;
maximbolduc 47:d3123bb4f673 171 char *asteer_speed = (char *)NULL;
maximbolduc 47:d3123bb4f673 172 char *asteer_time = (char *)NULL;
maximbolduc 47:d3123bb4f673 173 token = strtok(asteer, ",");
maximbolduc 47:d3123bb4f673 174 while (token) {
maximbolduc 47:d3123bb4f673 175 switch (token_counter) {
maximbolduc 47:d3123bb4f673 176 case 1:
maximbolduc 47:d3123bb4f673 177 asteer_speed = token;
maximbolduc 47:d3123bb4f673 178 break;
maximbolduc 47:d3123bb4f673 179 case 2:
maximbolduc 47:d3123bb4f673 180 asteer_time = token;
maximbolduc 47:d3123bb4f673 181 break;
maximbolduc 47:d3123bb4f673 182 }
maximbolduc 47:d3123bb4f673 183 token = strtok((char *)NULL, ",");
maximbolduc 47:d3123bb4f673 184 token_counter++;
maximbolduc 35:f9caeb8ca31e 185 }
maximbolduc 47:d3123bb4f673 186 if ( asteer_speed && asteer_time ) {
maximbolduc 47:d3123bb4f673 187 motorspeed = atof(asteer_speed);
maximbolduc 47:d3123bb4f673 188 enable_time = atof(asteer_time);
maximbolduc 47:d3123bb4f673 189 autosteer_timeout.reset();
maximbolduc 47:d3123bb4f673 190 time_till_stop = atoi(asteer_time);
maximbolduc 47:d3123bb4f673 191 if ( motor_enable == 0 ) {
maximbolduc 47:d3123bb4f673 192 } else {
maximbolduc 47:d3123bb4f673 193 if ( motorspeed > 127.0 ) {
maximbolduc 47:d3123bb4f673 194 pwm2_speed = 0.0;
maximbolduc 47:d3123bb4f673 195 pwm1_speed = ((double)motorspeed - (double)127.0) / 128.0;
maximbolduc 37:ac60a8a0ae8a 196
maximbolduc 47:d3123bb4f673 197 } else if ( motorspeed < 127.0 ) {
maximbolduc 47:d3123bb4f673 198 pwm1_speed = 0.0;
maximbolduc 47:d3123bb4f673 199 pwm2_speed = ( ((double)127-(double)motorspeed) / 128.0 );
maximbolduc 47:d3123bb4f673 200 } else {
maximbolduc 47:d3123bb4f673 201 pwm1_speed = 0;
maximbolduc 47:d3123bb4f673 202 pwm2_speed = 0;
maximbolduc 47:d3123bb4f673 203 enable_motor = 0;
maximbolduc 47:d3123bb4f673 204 }
maximbolduc 47:d3123bb4f673 205 // if(Authenticated)
maximbolduc 47:d3123bb4f673 206 // {
maximbolduc 47:d3123bb4f673 207 pwm1 = pwm1_speed;
maximbolduc 47:d3123bb4f673 208 pwm2 = pwm2_speed;
maximbolduc 47:d3123bb4f673 209 enable_motor = 1;
maximbolduc 47:d3123bb4f673 210 //}
maximbolduc 37:ac60a8a0ae8a 211 }
maximbolduc 35:f9caeb8ca31e 212 }
maximbolduc 35:f9caeb8ca31e 213 }
maximbolduc 41:a26acd346c2f 214 }
maximbolduc 41:a26acd346c2f 215
maximbolduc 38:b5352d6f8166 216
maximbolduc 48:5d9c63364c94 217 void action_on_rmc()
maximbolduc 26:dc00998140af 218 {
maximbolduc 39:6767d4c840f9 219 double angle = ((int)((track-90 )* -1 + 360) + 180) % 360 - 180;
maximbolduc 39:6767d4c840f9 220 double diff_angle = Freepilot_bearing - angle;
maximbolduc 39:6767d4c840f9 221 diff_angle = ((int)diff_angle + 180) % 360 - 180;
maximbolduc 39:6767d4c840f9 222
maximbolduc 39:6767d4c840f9 223 // pc.printf("%f %f %f\r\n",diff_angle, Freepilot_bearing, (track - 90) * -1);
maximbolduc 39:6767d4c840f9 224 if ( abs(diff_angle) > 90 ) {
maximbolduc 39:6767d4c840f9 225 if ( (abs(360 - diff_angle)) > 90 ) {
maximbolduc 39:6767d4c840f9 226 Point temp = line_end;
maximbolduc 39:6767d4c840f9 227 line_end = line_start;
maximbolduc 39:6767d4c840f9 228 line_start = temp;
maximbolduc 39:6767d4c840f9 229 Freepilot_bearing = ((int) Freepilot_bearing + 360) % 360 - 180;
maximbolduc 39:6767d4c840f9 230 }
maximbolduc 39:6767d4c840f9 231 }
maximbolduc 48:5d9c63364c94 232 cm_per_deg_lat = 11054000;
maximbolduc 48:5d9c63364c94 233 cm_per_deg_lon = 11132000 * cos(decimal_latitude);
maximbolduc 48:5d9c63364c94 234
maximbolduc 48:5d9c63364c94 235 pitch_and_roll((track-90)*-1);// as to be real bearing
maximbolduc 35:f9caeb8ca31e 236
maximbolduc 48:5d9c63364c94 237 compensation.SetY(compensation.GetY() / cm_per_deg_lon);
maximbolduc 48:5d9c63364c94 238 compensation.SetX(compensation.GetX() / cm_per_deg_lat);
maximbolduc 35:f9caeb8ca31e 239
maximbolduc 48:5d9c63364c94 240 position = point_add(position,compensation);
maximbolduc 35:f9caeb8ca31e 241
maximbolduc 48:5d9c63364c94 242 double lookaheaddistance = lookaheadtime * speed_m_s;
maximbolduc 48:5d9c63364c94 243 get_latlon_byangle(position.GetX(),position.GetY(),lookaheaddistance,(track-90)*-1,look_ahead_lon,look_ahead_lat);
maximbolduc 48:5d9c63364c94 244 looked_ahead.SetX(look_ahead_lat);
maximbolduc 48:5d9c63364c94 245 looked_ahead.SetY(look_ahead_lon);
maximbolduc 48:5d9c63364c94 246 double filtering = sqrt(111111.0*111111.0 + 111111.0 * cos(decimal_latitude/57.295779513)*111111.0 * cos(decimal_latitude/57.295779513));
maximbolduc 48:5d9c63364c94 247 distance_to_line = dist_Point_to_Line( looked_ahead,line_start,line_end) * isLeft( line_start,line_end,looked_ahead) * filtering;/////////////////////////////////////////////////
maximbolduc 35:f9caeb8ca31e 248
maximbolduc 48:5d9c63364c94 249 SetDigitalFilter(phaseadv,tcenter, 0 );
maximbolduc 48:5d9c63364c94 250 string steering = ControlSteerFilter(225, distance_to_line, speed_km , filterg, 20.0, 75.0, scale);
maximbolduc 35:f9caeb8ca31e 251
maximbolduc 48:5d9c63364c94 252 char command[128];
maximbolduc 48:5d9c63364c94 253 sprintf(command,"%s\r\n\0",steering.c_str()); //(int)((((val/2)-127)/scale)+127),500);
maximbolduc 48:5d9c63364c94 254 pc.puts(command);
maximbolduc 41:a26acd346c2f 255
maximbolduc 48:5d9c63364c94 256 process_ASTEER(command,false);
maximbolduc 48:5d9c63364c94 257
maximbolduc 34:c2bc9f9be7ff 258 string rmc = "";
maximbolduc 48:5d9c63364c94 259
maximbolduc 48:5d9c63364c94 260 rmc = "$GPRMC,";
maximbolduc 48:5d9c63364c94 261 rmc += string(time_s) + ",";
maximbolduc 48:5d9c63364c94 262 rmc +=(string(stat) + ",");
maximbolduc 48:5d9c63364c94 263 rmc +=( string(To_DMS(position.GetX())) + "," + string(lat_dir) +",");
maximbolduc 48:5d9c63364c94 264 rmc += (string(To_DMS_lon(position.GetY())) + "," + string(lon_dir) + ",");
maximbolduc 48:5d9c63364c94 265 rmc += (string(vel) + ",");
maximbolduc 48:5d9c63364c94 266 rmc += string(trk) + ",";
maximbolduc 48:5d9c63364c94 267 rmc += string(date) + ",";
maximbolduc 48:5d9c63364c94 268 rmc += string(magv) + ",";
maximbolduc 48:5d9c63364c94 269 rmc += string(magd) + ",W";
maximbolduc 34:c2bc9f9be7ff 270
maximbolduc 34:c2bc9f9be7ff 271 char test[256];
maximbolduc 38:b5352d6f8166 272 sprintf(test,"%s*\0",rmc.c_str());
maximbolduc 38:b5352d6f8166 273 sprintf(output,"%s*%02X\r\n\0",rmc.c_str(),getCheckSum(test));
maximbolduc 39:6767d4c840f9 274
maximbolduc 34:c2bc9f9be7ff 275 bluetooth.puts(output);
maximbolduc 26:dc00998140af 276 }
maximbolduc 35:f9caeb8ca31e 277
maximbolduc 26:dc00998140af 278 void process_FGPSAB(char* ab)
maximbolduc 26:dc00998140af 279 {
maximbolduc 33:3e71c418e90d 280 char *token;
maximbolduc 33:3e71c418e90d 281 int token_counter = 0;
maximbolduc 33:3e71c418e90d 282 char *line_lat = (char *)NULL;
maximbolduc 33:3e71c418e90d 283 char *line_lon = (char *)NULL;
maximbolduc 33:3e71c418e90d 284 char *line_lat1 = (char *)NULL;
maximbolduc 33:3e71c418e90d 285 char *line_lon1 = (char *)NULL;
maximbolduc 38:b5352d6f8166 286 //char *bearing = (char *)NULL;
maximbolduc 38:b5352d6f8166 287 string bearing = "";
maximbolduc 33:3e71c418e90d 288 token = strtok(ab, ",");
maximbolduc 35:f9caeb8ca31e 289 while (token) {
maximbolduc 35:f9caeb8ca31e 290 switch (token_counter) {
maximbolduc 33:3e71c418e90d 291 case 1:
maximbolduc 33:3e71c418e90d 292 line_lat = token;
maximbolduc 33:3e71c418e90d 293 break;
maximbolduc 33:3e71c418e90d 294 case 2:
maximbolduc 33:3e71c418e90d 295 line_lon = token;
maximbolduc 33:3e71c418e90d 296 break;
maximbolduc 33:3e71c418e90d 297 case 3:
maximbolduc 33:3e71c418e90d 298 line_lat1 = token;
maximbolduc 33:3e71c418e90d 299 break;
maximbolduc 33:3e71c418e90d 300 case 4:
maximbolduc 33:3e71c418e90d 301 line_lon1 = token;
maximbolduc 33:3e71c418e90d 302 break;
maximbolduc 33:3e71c418e90d 303 case 5:
maximbolduc 45:ecd8c2e27948 304 bearing = token;
maximbolduc 33:3e71c418e90d 305 break;
maximbolduc 26:dc00998140af 306 }
maximbolduc 33:3e71c418e90d 307 token = strtok((char *)NULL, ",");
maximbolduc 33:3e71c418e90d 308 token_counter++;
maximbolduc 33:3e71c418e90d 309 }
maximbolduc 39:6767d4c840f9 310 double Freepilot_lon = atof(line_lon);
maximbolduc 39:6767d4c840f9 311 double Freepilot_lat = atof(line_lat);
maximbolduc 39:6767d4c840f9 312 double Freepilot_lon1 = atof(line_lon1);
maximbolduc 39:6767d4c840f9 313 double Freepilot_lat1 = atof(line_lat1);
maximbolduc 39:6767d4c840f9 314 Freepilot_bearing = atof(bearing.c_str()) + 360;
maximbolduc 39:6767d4c840f9 315 Freepilot_bearing = ((int)Freepilot_bearing+ 180) % 360 - 180;
maximbolduc 33:3e71c418e90d 316 line_start.SetX(Freepilot_lat);
maximbolduc 33:3e71c418e90d 317 line_start.SetY(Freepilot_lon);
maximbolduc 33:3e71c418e90d 318 line_end.SetX(Freepilot_lat1);
maximbolduc 33:3e71c418e90d 319 line_end.SetY(Freepilot_lon1);
maximbolduc 39:6767d4c840f9 320
maximbolduc 33:3e71c418e90d 321 active_AB = 1;
maximbolduc 26:dc00998140af 322 }
maximbolduc 35:f9caeb8ca31e 323
maximbolduc 26:dc00998140af 324 void process_FGPSAUTO(char* FGPSAUTO)
maximbolduc 26:dc00998140af 325 {
maximbolduc 26:dc00998140af 326 char *token;
maximbolduc 26:dc00998140af 327 int token_counter = 0;
maximbolduc 26:dc00998140af 328 char *ahead = (char *)NULL;
maximbolduc 26:dc00998140af 329 char *center = (char *)NULL;
maximbolduc 26:dc00998140af 330 char *phase = (char *)NULL;
maximbolduc 26:dc00998140af 331 char *scl = (char *)NULL;
maximbolduc 26:dc00998140af 332 char *avg = (char *)NULL;
maximbolduc 26:dc00998140af 333 char *_kp = (char *)NULL;
maximbolduc 26:dc00998140af 334 char *_ki = (char *)NULL;
maximbolduc 35:f9caeb8ca31e 335 char *fg = (char *)NULL;
maximbolduc 26:dc00998140af 336 char *_kd = (char *)NULL;
maximbolduc 39:6767d4c840f9 337
maximbolduc 26:dc00998140af 338 token = strtok(FGPSAUTO, ",");
maximbolduc 35:f9caeb8ca31e 339 while (token) {
maximbolduc 35:f9caeb8ca31e 340 switch (token_counter) {
maximbolduc 26:dc00998140af 341 case 1:
maximbolduc 26:dc00998140af 342 phase = token;
maximbolduc 26:dc00998140af 343 break;
maximbolduc 26:dc00998140af 344 case 2:
maximbolduc 26:dc00998140af 345 center = token;
maximbolduc 26:dc00998140af 346 break;
maximbolduc 34:c2bc9f9be7ff 347 case 3:
maximbolduc 34:c2bc9f9be7ff 348 fg = token;
maximbolduc 34:c2bc9f9be7ff 349 break;
maximbolduc 26:dc00998140af 350 case 4:
maximbolduc 26:dc00998140af 351 scl = token;
maximbolduc 26:dc00998140af 352 break;
maximbolduc 26:dc00998140af 353 case 5:
maximbolduc 26:dc00998140af 354 ahead = token;
maximbolduc 26:dc00998140af 355 break;
maximbolduc 26:dc00998140af 356 case 6:
maximbolduc 26:dc00998140af 357 avg = token;
maximbolduc 26:dc00998140af 358 break;
maximbolduc 26:dc00998140af 359 case 7:
maximbolduc 26:dc00998140af 360 _kp = token;
maximbolduc 26:dc00998140af 361 break;
maximbolduc 26:dc00998140af 362 case 8:
maximbolduc 26:dc00998140af 363 _ki = token;
maximbolduc 26:dc00998140af 364 break;
maximbolduc 26:dc00998140af 365 case 9:
maximbolduc 26:dc00998140af 366 _kd = token;
maximbolduc 26:dc00998140af 367 break;
maximbolduc 26:dc00998140af 368 }
maximbolduc 26:dc00998140af 369 token = strtok((char *)NULL, ",");
maximbolduc 26:dc00998140af 370 token_counter++;
maximbolduc 26:dc00998140af 371 }
maximbolduc 35:f9caeb8ca31e 372 if ( _kp && _ki && _kd ) {
maximbolduc 26:dc00998140af 373 kp = atof(_kp);
maximbolduc 26:dc00998140af 374 ki = atof(_ki);
maximbolduc 26:dc00998140af 375 kd = atof(_kd);
maximbolduc 26:dc00998140af 376 }
maximbolduc 35:f9caeb8ca31e 377 if ( phase && center && scl && avg && ahead ) {
jhedmonton 28:5905886c76ee 378 lookaheadtime = atof(ahead);
maximbolduc 41:a26acd346c2f 379 scale = atof(scl);
maximbolduc 26:dc00998140af 380 phaseadv = atof(phase);
jhedmonton 28:5905886c76ee 381 avgpos = atof(avg);
jhedmonton 28:5905886c76ee 382 tcenter = atof(center);
maximbolduc 34:c2bc9f9be7ff 383 filterg = atof(fg);
maximbolduc 38:b5352d6f8166 384
maximbolduc 41:a26acd346c2f 385 // scale = scale * -1;
maximbolduc 41:a26acd346c2f 386 SetDigitalFilter(phaseadv,tcenter, 0 );
maximbolduc 26:dc00998140af 387 }
maximbolduc 26:dc00998140af 388 }
maximbolduc 35:f9caeb8ca31e 389
maximbolduc 26:dc00998140af 390 void pc_analyse(char* pc_string)
maximbolduc 26:dc00998140af 391 {
maximbolduc 41:a26acd346c2f 392 pc.puts(pc_string);
maximbolduc 35:f9caeb8ca31e 393 if (!strncmp(pc_string, "$ASTEER", 7)) {
maximbolduc 39:6767d4c840f9 394 //stop sending when already exists
maximbolduc 46:d7d6dc429153 395 process_ASTEER(pc_string,true);
maximbolduc 35:f9caeb8ca31e 396 } else if (!strncmp(pc_string, "$BANY",5)) {
jhedmonton 29:23ccb2a50b6f 397 _ID = Config_GetID();
maximbolduc 42:854d8cc26bbb 398 // Config_Save();
maximbolduc 35:f9caeb8ca31e 399 } else if (!strncmp(pc_string, "$GPSBAUD",8)) {
maximbolduc 45:ecd8c2e27948 400
maximbolduc 45:ecd8c2e27948 401 process_GPSBAUD(pc_string);
maximbolduc 42:854d8cc26bbb 402 // Config_Save();
maximbolduc 35:f9caeb8ca31e 403 } else if (!strncmp(pc_string, "$FGPSAUTO",9)) {
maximbolduc 34:c2bc9f9be7ff 404 process_FGPSAUTO(pc_string);
maximbolduc 34:c2bc9f9be7ff 405 sprintf(output,"%s\r\n",pc_string);
maximbolduc 34:c2bc9f9be7ff 406 bluetooth.puts(output);
maximbolduc 42:854d8cc26bbb 407 // Config_Save();
maximbolduc 35:f9caeb8ca31e 408 } else if (!strncmp(pc_string, "$FGPS,",6)) {
maximbolduc 35:f9caeb8ca31e 409 int i=5;
maximbolduc 35:f9caeb8ca31e 410 char c=pc_string[i];
maximbolduc 35:f9caeb8ca31e 411 while (c!=0) {
maximbolduc 35:f9caeb8ca31e 412 i++;
maximbolduc 35:f9caeb8ca31e 413 if (i>255) break; //protect msg buffer!
maximbolduc 35:f9caeb8ca31e 414 c=pc_string[i];
maximbolduc 35:f9caeb8ca31e 415 gps.putc(c);
maximbolduc 35:f9caeb8ca31e 416 pc.putc(c);
maximbolduc 35:f9caeb8ca31e 417 }
maximbolduc 36:8e84b5ade03e 418 } else if (!strncmp(pc_string, "$GPSHEIGHT",10)) {
maximbolduc 26:dc00998140af 419 process_GPSHEIGHT(pc_string);
maximbolduc 26:dc00998140af 420 sprintf(output,"%s\r\n",pc_string);
maximbolduc 26:dc00998140af 421 bluetooth.puts(output);
maximbolduc 42:854d8cc26bbb 422 // Config_Save();
maximbolduc 37:ac60a8a0ae8a 423 } else if (!strncmp(pc_string, "$FGPSAB",7)) {
maximbolduc 26:dc00998140af 424 process_FGPSAB(pc_string);
maximbolduc 47:d3123bb4f673 425 } else if(strcmp(pc_string, "$V2,1") == 0) {
maximbolduc 46:d7d6dc429153 426 freepilot_v2 = true;
maximbolduc 47:d3123bb4f673 427 } else if(strcmp(pc_string, "$V2,0") == 0) {
maximbolduc 46:d7d6dc429153 428 freepilot_v2 = false;
maximbolduc 35:f9caeb8ca31e 429 } else if (!strncmp(pc_string, "$CALIBRATEACCEL",15)) {
maximbolduc 32:c57bc701d65c 430 calibrateGyroscope();
maximbolduc 30:3afafa1ef16b 431 calibrateAccelerometer();
maximbolduc 42:854d8cc26bbb 432 // Config_Save();
maximbolduc 35:f9caeb8ca31e 433 } else if (!strncmp(pc_string, "$POSITION",9)) {
maximbolduc 32:c57bc701d65c 434 char* pointer;
maximbolduc 32:c57bc701d65c 435 char* Data[5];
maximbolduc 32:c57bc701d65c 436 int index = 0;
maximbolduc 32:c57bc701d65c 437 //Split data at commas
maximbolduc 32:c57bc701d65c 438 pointer = strtok(pc_string, ",");
maximbolduc 32:c57bc701d65c 439 if(pointer == NULL)
maximbolduc 32:c57bc701d65c 440 Data[0] = pc_string;
maximbolduc 35:f9caeb8ca31e 441 while(pointer != NULL) {
maximbolduc 32:c57bc701d65c 442 Data[index] = pointer;
maximbolduc 32:c57bc701d65c 443 pointer = strtok(NULL, ",");
maximbolduc 32:c57bc701d65c 444 index++;
maximbolduc 32:c57bc701d65c 445 }
maximbolduc 32:c57bc701d65c 446 gyro_pos = atoi(Data[1]);
maximbolduc 42:854d8cc26bbb 447 // Config_Save();
maximbolduc 35:f9caeb8ca31e 448 } else {
maximbolduc 26:dc00998140af 449 }
maximbolduc 26:dc00998140af 450 }
maximbolduc 35:f9caeb8ca31e 451
maximbolduc 26:dc00998140af 452 void gps_analyse(char* gps_string)
maximbolduc 26:dc00998140af 453 {
maximbolduc 35:f9caeb8ca31e 454 if (!strncmp(gps_string, "$GPRMC", 6)) {
maximbolduc 48:5d9c63364c94 455 if (nmea_rmc(gps_string)) {
maximbolduc 48:5d9c63364c94 456 void action_on_rmc();
maximbolduc 48:5d9c63364c94 457 } //analyse and decompose the rmc string
maximbolduc 36:8e84b5ade03e 458 } else {
maximbolduc 36:8e84b5ade03e 459 bluetooth.puts(gps_string);
maximbolduc 33:3e71c418e90d 460 }
maximbolduc 26:dc00998140af 461 }
maximbolduc 35:f9caeb8ca31e 462
jhedmonton 27:9ac59b261d87 463 int i2 = 0;
jhedmonton 27:9ac59b261d87 464 bool end2 = false;
jhedmonton 27:9ac59b261d87 465 bool start2 = false;
maximbolduc 35:f9caeb8ca31e 466
jhedmonton 27:9ac59b261d87 467 bool getline2()
maximbolduc 26:dc00998140af 468 {
jhedmonton 27:9ac59b261d87 469 int gotstring=false;
maximbolduc 35:f9caeb8ca31e 470 while (1) {
maximbolduc 35:f9caeb8ca31e 471 if( !bluetooth.readable() ) {
jhedmonton 27:9ac59b261d87 472 break;
jhedmonton 27:9ac59b261d87 473 }
jhedmonton 27:9ac59b261d87 474 char c = bluetooth.getc();
maximbolduc 35:f9caeb8ca31e 475 if (c == 36 ) {
maximbolduc 33:3e71c418e90d 476 start2=true;
maximbolduc 33:3e71c418e90d 477 end2 = false;
maximbolduc 33:3e71c418e90d 478 i2 = 0;
maximbolduc 33:3e71c418e90d 479 }
maximbolduc 35:f9caeb8ca31e 480 if ((start2) && (c == 10)) {
maximbolduc 33:3e71c418e90d 481 end2=true;
jhedmonton 29:23ccb2a50b6f 482 start2 = false;
jhedmonton 29:23ccb2a50b6f 483 }
maximbolduc 35:f9caeb8ca31e 484 if (start2) {
jhedmonton 27:9ac59b261d87 485 msg2[i2]=c;
jhedmonton 27:9ac59b261d87 486 i2++;
jhedmonton 27:9ac59b261d87 487 if (i2>255) break; //protect msg buffer!
jhedmonton 27:9ac59b261d87 488 }
maximbolduc 35:f9caeb8ca31e 489 if (end2) {
maximbolduc 33:3e71c418e90d 490 msg2[i2]=c;
jhedmonton 27:9ac59b261d87 491 msg2[i2+1] = 0;
jhedmonton 27:9ac59b261d87 492 start2 = false;
jhedmonton 27:9ac59b261d87 493 gotstring = true;
jhedmonton 27:9ac59b261d87 494 end2=false;
jhedmonton 27:9ac59b261d87 495 i2=0;
jhedmonton 27:9ac59b261d87 496 break;
maximbolduc 26:dc00998140af 497 }
maximbolduc 26:dc00998140af 498 }
jhedmonton 27:9ac59b261d87 499 return gotstring;
maximbolduc 26:dc00998140af 500 }
maximbolduc 35:f9caeb8ca31e 501
maximbolduc 35:f9caeb8ca31e 502
jhedmonton 27:9ac59b261d87 503 int i=0;
jhedmonton 27:9ac59b261d87 504 bool start=false;
jhedmonton 27:9ac59b261d87 505 bool end=false;
maximbolduc 35:f9caeb8ca31e 506
jhedmonton 27:9ac59b261d87 507 bool getline(bool forward)
maximbolduc 26:dc00998140af 508 {
maximbolduc 35:f9caeb8ca31e 509 while (1) {
maximbolduc 35:f9caeb8ca31e 510 if( !gps.readable() ) {
jhedmonton 27:9ac59b261d87 511 break;
jhedmonton 27:9ac59b261d87 512 }
jhedmonton 28:5905886c76ee 513 char c = gps.getc();
maximbolduc 35:f9caeb8ca31e 514 if (forward) { //simply forward all to Bluetooth
maximbolduc 35:f9caeb8ca31e 515 bluetooth.putc(c);
jhedmonton 27:9ac59b261d87 516 }
maximbolduc 35:f9caeb8ca31e 517 if (c == 36 ) {
maximbolduc 35:f9caeb8ca31e 518 start=true;
maximbolduc 35:f9caeb8ca31e 519 end = false;
maximbolduc 35:f9caeb8ca31e 520 i = 0;
maximbolduc 35:f9caeb8ca31e 521 }
maximbolduc 35:f9caeb8ca31e 522 if ((start) && (c == 10)) {
maximbolduc 35:f9caeb8ca31e 523 end=true;
jhedmonton 29:23ccb2a50b6f 524 start = false;
jhedmonton 29:23ccb2a50b6f 525 }
maximbolduc 35:f9caeb8ca31e 526 if (start) {
jhedmonton 27:9ac59b261d87 527 msg[i]=c;
jhedmonton 27:9ac59b261d87 528 i++;
jhedmonton 27:9ac59b261d87 529 if (i>255) break; //protect msg buffer!
jhedmonton 27:9ac59b261d87 530 }
maximbolduc 35:f9caeb8ca31e 531 if (end) {
maximbolduc 35:f9caeb8ca31e 532 msg[i]=c;
maximbolduc 26:dc00998140af 533 msg[i+1] = 0;
jhedmonton 29:23ccb2a50b6f 534 i=0;
jhedmonton 27:9ac59b261d87 535 start = false;
jhedmonton 27:9ac59b261d87 536 end = true;
jhedmonton 27:9ac59b261d87 537 break;
maximbolduc 26:dc00998140af 538 }
maximbolduc 26:dc00998140af 539 }
jhedmonton 27:9ac59b261d87 540 return end;
maximbolduc 26:dc00998140af 541 }
maximbolduc 35:f9caeb8ca31e 542
maximbolduc 26:dc00998140af 543 void keyPressedHeld( void )
maximbolduc 26:dc00998140af 544 {
maximbolduc 36:8e84b5ade03e 545 motor_enable_state = "$ENABLE,0\r\n";
maximbolduc 37:ac60a8a0ae8a 546 motor_enable = 0;
maximbolduc 37:ac60a8a0ae8a 547 pwm1 = 0.0;
maximbolduc 37:ac60a8a0ae8a 548 pwm2 = 0.0;
jhedmonton 28:5905886c76ee 549 ledGREEN=1; //show green for being ready to steer
maximbolduc 26:dc00998140af 550 }
maximbolduc 35:f9caeb8ca31e 551
maximbolduc 26:dc00998140af 552 void keyReleasedHeld( void )
maximbolduc 26:dc00998140af 553 {
maximbolduc 34:c2bc9f9be7ff 554 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
maximbolduc 34:c2bc9f9be7ff 555 motor_enable_state = "$ENABLE,1\r\n";
maximbolduc 37:ac60a8a0ae8a 556 motor_enable = 1;
maximbolduc 37:ac60a8a0ae8a 557 pwm1 = 0.0;
maximbolduc 37:ac60a8a0ae8a 558 pwm2 = 0.0;
maximbolduc 35:f9caeb8ca31e 559 ledGREEN=0;
jhedmonton 27:9ac59b261d87 560 }
maximbolduc 35:f9caeb8ca31e 561
jhedmonton 27:9ac59b261d87 562 void boom1PressedHeld( void )
jhedmonton 27:9ac59b261d87 563 {
maximbolduc 35:f9caeb8ca31e 564 // ledGREEN=1;
maximbolduc 35:f9caeb8ca31e 565 boom18=boom18 & 0xFE;
jhedmonton 27:9ac59b261d87 566 }
maximbolduc 35:f9caeb8ca31e 567
jhedmonton 27:9ac59b261d87 568 void boom1ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 569 {
maximbolduc 35:f9caeb8ca31e 570 //ledGREEN=0;
maximbolduc 35:f9caeb8ca31e 571 boom18=boom18 | 0x01;
jhedmonton 27:9ac59b261d87 572 }
maximbolduc 35:f9caeb8ca31e 573
jhedmonton 27:9ac59b261d87 574 void boom2PressedHeld( void )
maximbolduc 35:f9caeb8ca31e 575 {
maximbolduc 35:f9caeb8ca31e 576 boom18=boom18 & 0xFD;
maximbolduc 26:dc00998140af 577 }
maximbolduc 35:f9caeb8ca31e 578
jhedmonton 27:9ac59b261d87 579 void boom2ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 580 {
maximbolduc 35:f9caeb8ca31e 581 boom18=boom18 | 0x02;
jhedmonton 27:9ac59b261d87 582 }
jhedmonton 27:9ac59b261d87 583 void boom3PressedHeld( void )
jhedmonton 27:9ac59b261d87 584 {
maximbolduc 35:f9caeb8ca31e 585 boom18=boom18 & 0xFB;
jhedmonton 27:9ac59b261d87 586 }
maximbolduc 35:f9caeb8ca31e 587
jhedmonton 27:9ac59b261d87 588 void boom3ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 589 {
maximbolduc 35:f9caeb8ca31e 590 boom18=boom18 | 0x04;
jhedmonton 27:9ac59b261d87 591 }
maximbolduc 35:f9caeb8ca31e 592
jhedmonton 27:9ac59b261d87 593 void boom4PressedHeld( void )
jhedmonton 27:9ac59b261d87 594 {
maximbolduc 32:c57bc701d65c 595 boom18=boom18 & 0xF7;
jhedmonton 27:9ac59b261d87 596 }
maximbolduc 35:f9caeb8ca31e 597
jhedmonton 27:9ac59b261d87 598 void boom4ReleasedHeld( void )
jhedmonton 27:9ac59b261d87 599 {
maximbolduc 32:c57bc701d65c 600 boom18=boom18 | 0x08;
jhedmonton 27:9ac59b261d87 601 }
maximbolduc 35:f9caeb8ca31e 602
maximbolduc 26:dc00998140af 603 void toprint()
maximbolduc 26:dc00998140af 604 {
maximbolduc 26:dc00998140af 605 angle_send = 1;
maximbolduc 26:dc00998140af 606 }
maximbolduc 35:f9caeb8ca31e 607
maximbolduc 38:b5352d6f8166 608 double last_yaw = 0;
maximbolduc 38:b5352d6f8166 609 int counter = 0;
maximbolduc 38:b5352d6f8166 610 bool bear = false;
maximbolduc 38:b5352d6f8166 611 double lastroll = 0;
maximbolduc 38:b5352d6f8166 612 double lastpitch = 0;
maximbolduc 38:b5352d6f8166 613
maximbolduc 26:dc00998140af 614 int main()
maximbolduc 26:dc00998140af 615 {
jhedmonton 27:9ac59b261d87 616 bluetooth.baud(115200);
jhedmonton 28:5905886c76ee 617 gps.baud(38400);
maximbolduc 30:3afafa1ef16b 618 pc.baud(38400);
maximbolduc 35:f9caeb8ca31e 619
jhedmonton 27:9ac59b261d87 620 //JH prepare and send version info
jhedmonton 27:9ac59b261d87 621 vTimer.start();
jhedmonton 27:9ac59b261d87 622 vTimer.reset();
maximbolduc 38:b5352d6f8166 623
jhedmonton 28:5905886c76ee 624 motTimer.start();
jhedmonton 28:5905886c76ee 625 motTimer.reset();
jhedmonton 29:23ccb2a50b6f 626 lastsend_motorstate=motTimer.read_ms()-6500; //should trigger in 5s
maximbolduc 34:c2bc9f9be7ff 627 motor_enable_state = "$ENABLE,1\r\n";
maximbolduc 36:8e84b5ade03e 628
jhedmonton 28:5905886c76ee 629 btTimer.start();
jhedmonton 28:5905886c76ee 630 btTimer.reset();
maximbolduc 38:b5352d6f8166 631 lastgetBT= btTimer.read_ms();
maximbolduc 35:f9caeb8ca31e 632
jhedmonton 27:9ac59b261d87 633 bluetooth.puts(version);
maximbolduc 38:b5352d6f8166 634
jhedmonton 29:23ccb2a50b6f 635 lastsend_version=vTimer.read_ms()-18000;
maximbolduc 38:b5352d6f8166 636 // pc.puts("test\r\n");
maximbolduc 38:b5352d6f8166 637 /* Config_Startup();
maximbolduc 38:b5352d6f8166 638 _ID = Config_GetID();
maximbolduc 38:b5352d6f8166 639 Config_Save();
maximbolduc 38:b5352d6f8166 640 */
jhedmonton 27:9ac59b261d87 641 boom1.attach_asserted_held( &boom1PressedHeld );
jhedmonton 27:9ac59b261d87 642 boom1.attach_deasserted_held( &boom1ReleasedHeld );
jhedmonton 27:9ac59b261d87 643 boom1.setSampleFrequency(); //default = 20 ms
jhedmonton 27:9ac59b261d87 644 boom1.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 645 boom1.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 646 boom2.attach_asserted_held( &boom2PressedHeld );
jhedmonton 27:9ac59b261d87 647 boom2.attach_deasserted_held( &boom2ReleasedHeld );
jhedmonton 27:9ac59b261d87 648 boom2.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 649 boom2.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 650 boom2.setSampleFrequency();
jhedmonton 27:9ac59b261d87 651 boom3.attach_asserted_held( &boom3PressedHeld );
jhedmonton 27:9ac59b261d87 652 boom3.attach_deasserted_held( &boom3ReleasedHeld );
jhedmonton 27:9ac59b261d87 653 boom3.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 654 boom3.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 655 boom3.setSampleFrequency();
jhedmonton 27:9ac59b261d87 656 boom4.attach_asserted_held( &boom4PressedHeld );
jhedmonton 27:9ac59b261d87 657 boom4.attach_deasserted_held( &boom4ReleasedHeld );
jhedmonton 27:9ac59b261d87 658 boom4.setSamplesTillAssert(5);
jhedmonton 27:9ac59b261d87 659 boom4.setSamplesTillHeld(5);
jhedmonton 27:9ac59b261d87 660 boom4.setSampleFrequency();
maximbolduc 47:d3123bb4f673 661
maximbolduc 44:e9d5cd98273d 662 motor_switch.setSamplesTillAssert(5);
maximbolduc 44:e9d5cd98273d 663 motor_switch.setSamplesTillHeld(5);
maximbolduc 44:e9d5cd98273d 664 motor_switch.setSampleFrequency();
maximbolduc 26:dc00998140af 665 motor_switch.attach_asserted_held( &keyPressedHeld );
maximbolduc 26:dc00998140af 666 motor_switch.attach_deasserted_held( &keyReleasedHeld );
maximbolduc 35:f9caeb8ca31e 667
maximbolduc 36:8e84b5ade03e 668 initializeAccelerometer();
maximbolduc 36:8e84b5ade03e 669 calibrateAccelerometer();
maximbolduc 36:8e84b5ade03e 670 initializeGyroscope();
maximbolduc 36:8e84b5ade03e 671 calibrateGyroscope();
maximbolduc 36:8e84b5ade03e 672
maximbolduc 30:3afafa1ef16b 673 accelerometerTicker.attach(&sampleAccelerometer, 0.005);
maximbolduc 30:3afafa1ef16b 674 gyroscopeTicker.attach(&sampleGyroscope, 0.005);
maximbolduc 30:3afafa1ef16b 675 filterTicker.attach(&filter, FILTER_RATE);
maximbolduc 30:3afafa1ef16b 676 angle_print.attach(&toprint,0.2);
maximbolduc 26:dc00998140af 677 activate_antenna();
maximbolduc 36:8e84b5ade03e 678 autosteer_timeout.start();
maximbolduc 38:b5352d6f8166 679
maximbolduc 44:e9d5cd98273d 680 //setTunings(0.25, 5, 1); //for PID
maximbolduc 44:e9d5cd98273d 681 SetDigitalFilter(phaseadv,tcenter, 0); //for FGPS guidance
maximbolduc 38:b5352d6f8166 682
maximbolduc 35:f9caeb8ca31e 683 while(1) {
jhedmonton 27:9ac59b261d87 684 //JH send version information every 10 seconds to keep Bluetooth alive
maximbolduc 35:f9caeb8ca31e 685 if ((vTimer.read_ms()-lastsend_version)>25000) {
jhedmonton 27:9ac59b261d87 686 pc.puts(version);
jhedmonton 27:9ac59b261d87 687 bluetooth.puts(version);
jhedmonton 27:9ac59b261d87 688 vTimer.reset();
jhedmonton 27:9ac59b261d87 689 lastsend_version=vTimer.read_ms();
maximbolduc 35:f9caeb8ca31e 690 }
maximbolduc 35:f9caeb8ca31e 691
maximbolduc 36:8e84b5ade03e 692 if ( autosteer_timeout.read_ms() > 5000 || autosteer_timeout.read_ms() > time_till_stop ) {
maximbolduc 36:8e84b5ade03e 693 autosteer_timeout.reset();
maximbolduc 36:8e84b5ade03e 694 enable_motor = 0;
maximbolduc 36:8e84b5ade03e 695 }
maximbolduc 35:f9caeb8ca31e 696 if ( antenna_active == 1 && gps.readable()) {
maximbolduc 35:f9caeb8ca31e 697 if (getline(false)) {
maximbolduc 45:ecd8c2e27948 698 if ( validate_checksum(msg,result)) {
maximbolduc 38:b5352d6f8166 699 //pc.puts(msg);
maximbolduc 45:ecd8c2e27948 700 gps_analyse(result);
maximbolduc 35:f9caeb8ca31e 701 } else {
maximbolduc 33:3e71c418e90d 702 pc.puts("INVALID!!!!\r\n");
maximbolduc 32:c57bc701d65c 703 }
maximbolduc 26:dc00998140af 704 }
maximbolduc 26:dc00998140af 705 }
maximbolduc 35:f9caeb8ca31e 706 if ( bluetooth.readable()) {
maximbolduc 35:f9caeb8ca31e 707 if (getline2()) {
maximbolduc 45:ecd8c2e27948 708 if ( validate_checksum(msg2,result)) {
maximbolduc 45:ecd8c2e27948 709 btTimer.reset();
maximbolduc 45:ecd8c2e27948 710 lastgetBT= btTimer.read_ms();
maximbolduc 45:ecd8c2e27948 711 // pc.puts(msg2);
maximbolduc 45:ecd8c2e27948 712 pc_analyse(result);
maximbolduc 45:ecd8c2e27948 713 }
maximbolduc 26:dc00998140af 714 }
maximbolduc 26:dc00998140af 715 }
maximbolduc 35:f9caeb8ca31e 716 if ( btTimer.read_ms()-lastgetBT>1000) {
maximbolduc 35:f9caeb8ca31e 717 //we did not get any commands over BT
maximbolduc 35:f9caeb8ca31e 718 ledRED=1; //turn red
maximbolduc 35:f9caeb8ca31e 719 } else ledRED=0;
maximbolduc 35:f9caeb8ca31e 720
maximbolduc 35:f9caeb8ca31e 721 if ( ((motTimer.read_ms()-lastsend_motorstate)>8000) || (motor_enable!=lastmotor_enable)) {
maximbolduc 26:dc00998140af 722 bluetooth.puts(motor_enable_state);
maximbolduc 38:b5352d6f8166 723 // pc.puts(motor_enable_state);
jhedmonton 28:5905886c76ee 724 motTimer.reset();
jhedmonton 28:5905886c76ee 725 lastsend_motorstate=motTimer.read_ms();
jhedmonton 28:5905886c76ee 726 lastmotor_enable=motor_enable;
maximbolduc 26:dc00998140af 727 }
maximbolduc 35:f9caeb8ca31e 728 if (boom18!=lastboom18) {
maximbolduc 35:f9caeb8ca31e 729 boomstate[4]=boom18 | 0x80; //
maximbolduc 35:f9caeb8ca31e 730 bluetooth.puts(boomstate);
maximbolduc 38:b5352d6f8166 731 // pc.puts(boomstate);
maximbolduc 35:f9caeb8ca31e 732 lastboom18=boom18;
jhedmonton 27:9ac59b261d87 733 }
maximbolduc 44:e9d5cd98273d 734 if ( print_euler == 1 && angle_send == 1 ) {
maximbolduc 44:e9d5cd98273d 735 //&& reading == 0){
maximbolduc 43:e3f064f5eecd 736 lastpitch = 0.9*lastpitch + 0.1 * (toDegrees(get_pitch()*3.5));
maximbolduc 43:e3f064f5eecd 737 lastroll = 0.9 * lastroll + 0.1 * toDegrees(get_roll()*3.5);
maximbolduc 38:b5352d6f8166 738
maximbolduc 38:b5352d6f8166 739 double dps = - last_yaw;
maximbolduc 38:b5352d6f8166 740 last_yaw =toDegrees( imuFilter.getYaw()) * -1;
maximbolduc 38:b5352d6f8166 741 dps = (dps + last_yaw) * 5; // update every 200 ms, so for dps need *5
maximbolduc 38:b5352d6f8166 742
maximbolduc 38:b5352d6f8166 743 sprintf(output,"$EULER,%f,%f,%f\r\n",lastroll,lastpitch,dps);
maximbolduc 30:3afafa1ef16b 744 bluetooth.puts(output);
maximbolduc 26:dc00998140af 745 angle_send = 0;
maximbolduc 38:b5352d6f8166 746 counter++;
maximbolduc 38:b5352d6f8166 747 if ( bear == false && counter > 3 ) { //reinitialise the gyro after 600ms for the reference to be changed.
maximbolduc 38:b5352d6f8166 748 imuFilter.reset();
maximbolduc 38:b5352d6f8166 749 bear = true;
maximbolduc 38:b5352d6f8166 750 }
jhedmonton 27:9ac59b261d87 751 }
maximbolduc 26:dc00998140af 752 }
maximbolduc 35:f9caeb8ca31e 753 }