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@52:2b44e1b2f33b, 2015-03-14 (annotated)

Committer:
maximbolduc
Date:
Sat Mar 14 17:57:42 2015 +0000
Revision:
52:2b44e1b2f33b
Parent:
51:f874574674c0
Child:
53:7b17d99ba7ee
fixed motor direction

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