Dependencies:   mbed

Committer:
pd0wm
Date:
Tue Sep 27 19:46:30 2011 +0000
Revision:
0:bec310bde899

        

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pd0wm 0:bec310bde899 1 #include "status.h"
pd0wm 0:bec310bde899 2
pd0wm 0:bec310bde899 3
pd0wm 0:bec310bde899 4 Status::Status(Roer * r, Zeil * z, Kompas * k, Gps * g, Vaantje * v, Route * rout, Goto * gt, Pid * p) : zender(p13,p14) {
pd0wm 0:bec310bde899 5 roer = r;
pd0wm 0:bec310bde899 6 zeil = z;
pd0wm 0:bec310bde899 7 kompas = k;
pd0wm 0:bec310bde899 8 gps = g;
pd0wm 0:bec310bde899 9 route = rout;
pd0wm 0:bec310bde899 10 vaantje = v;
pd0wm 0:bec310bde899 11 got = gt;
pd0wm 0:bec310bde899 12 pid = p;
pd0wm 0:bec310bde899 13
pd0wm 0:bec310bde899 14 manual = 1;
pd0wm 0:bec310bde899 15 zeilmanual = 1;
pd0wm 0:bec310bde899 16
pd0wm 0:bec310bde899 17 zender.baud(115200);
pd0wm 0:bec310bde899 18 zender.attach(this, &Status::newData);
pd0wm 0:bec310bde899 19 }
pd0wm 0:bec310bde899 20
pd0wm 0:bec310bde899 21 void Status::autoZeil(void) {
pd0wm 0:bec310bde899 22 double hoek = fabs(verschil((double)kompas->get(), (double)vaantje->get()));
pd0wm 0:bec310bde899 23 int s;
pd0wm 0:bec310bde899 24 if (hoek <= 40)
pd0wm 0:bec310bde899 25 s = 35;
pd0wm 0:bec310bde899 26 else if (hoek > 40 && hoek <= 100)
pd0wm 0:bec310bde899 27 s = (hoek - 40) * (55.0/60.0) + 35;
pd0wm 0:bec310bde899 28 else
pd0wm 0:bec310bde899 29 s = 90;
pd0wm 0:bec310bde899 30 zeil->set(s);
pd0wm 0:bec310bde899 31 }
pd0wm 0:bec310bde899 32
pd0wm 0:bec310bde899 33 void Status::newData(void) {
pd0wm 0:bec310bde899 34
pd0wm 0:bec310bde899 35 //buffer variables
pd0wm 0:bec310bde899 36 char lezen;
pd0wm 0:bec310bde899 37
pd0wm 0:bec310bde899 38 //if data is ready in the buffer
pd0wm 0:bec310bde899 39 while (zender.readable()) {
pd0wm 0:bec310bde899 40 //read 1 character
pd0wm 0:bec310bde899 41 lezen = zender.getc();
pd0wm 0:bec310bde899 42
pd0wm 0:bec310bde899 43 //if character is $ than it is the start of a sentence
pd0wm 0:bec310bde899 44 if (lezen == '$') {
pd0wm 0:bec310bde899 45 //so the pointer should be set to the first position
pd0wm 0:bec310bde899 46 b.clear();
pd0wm 0:bec310bde899 47
pd0wm 0:bec310bde899 48 }
pd0wm 0:bec310bde899 49 //write buffer character to big buffer string
pd0wm 0:bec310bde899 50 b += lezen;
pd0wm 0:bec310bde899 51
pd0wm 0:bec310bde899 52 //if the character is # than the end of the sentence is reached and some stuff has to be done
pd0wm 0:bec310bde899 53 if (lezen == '#') {
pd0wm 0:bec310bde899 54
pd0wm 0:bec310bde899 55 char * cstr, *p;
pd0wm 0:bec310bde899 56 string c1,c2,c3,c4;
pd0wm 0:bec310bde899 57 int set = 1;
pd0wm 0:bec310bde899 58 int n;
pd0wm 0:bec310bde899 59 float f;
pd0wm 0:bec310bde899 60
pd0wm 0:bec310bde899 61 //start en stop karakters afhakken
pd0wm 0:bec310bde899 62 b.erase(0,1);
pd0wm 0:bec310bde899 63 b.erase(b.length()-1,1);
pd0wm 0:bec310bde899 64
pd0wm 0:bec310bde899 65 cstr = new char [b.size()+1];
pd0wm 0:bec310bde899 66 strcpy (cstr, b.c_str());
pd0wm 0:bec310bde899 67
pd0wm 0:bec310bde899 68 p = strtok(cstr," ");
pd0wm 0:bec310bde899 69 if (p != NULL)
pd0wm 0:bec310bde899 70 c1 = p;
pd0wm 0:bec310bde899 71 p = strtok(NULL," ");
pd0wm 0:bec310bde899 72 if (p != NULL)
pd0wm 0:bec310bde899 73 c2 = p;
pd0wm 0:bec310bde899 74 p = strtok(NULL," ");
pd0wm 0:bec310bde899 75 if (p != NULL)
pd0wm 0:bec310bde899 76 c3 = p;
pd0wm 0:bec310bde899 77 else
pd0wm 0:bec310bde899 78 set = 0;
pd0wm 0:bec310bde899 79
pd0wm 0:bec310bde899 80 p = strtok(NULL," ");
pd0wm 0:bec310bde899 81 if (p != NULL)
pd0wm 0:bec310bde899 82 c4 = p;
pd0wm 0:bec310bde899 83
pd0wm 0:bec310bde899 84 delete[] cstr;
pd0wm 0:bec310bde899 85
pd0wm 0:bec310bde899 86 if (c1 == "s") {
pd0wm 0:bec310bde899 87 zender.printf("%d %d %d %f %f %d %f %f %d\n\r",roer->get(),zeil->get(),kompas->get(),gps->get().lat,gps->get().lon,route->getActive(),route->afstand_tot_doel(), route->koers_tot_doel(),zeilmanual);
pd0wm 0:bec310bde899 88
pd0wm 0:bec310bde899 89 }
pd0wm 0:bec310bde899 90
pd0wm 0:bec310bde899 91 else if (c1 == "roer") {
pd0wm 0:bec310bde899 92 if (c2 == "set") {
pd0wm 0:bec310bde899 93 if (set) {
pd0wm 0:bec310bde899 94 sscanf (c3.c_str(),"%d",&n);
pd0wm 0:bec310bde899 95 roer->set(n);
pd0wm 0:bec310bde899 96 }
pd0wm 0:bec310bde899 97 } else if (c2 == "get") {
pd0wm 0:bec310bde899 98 if (!set) {
pd0wm 0:bec310bde899 99 zender.printf("%d\n\r",roer->get());
pd0wm 0:bec310bde899 100 }
pd0wm 0:bec310bde899 101 }
pd0wm 0:bec310bde899 102 } else if (c1 == "zeil") {
pd0wm 0:bec310bde899 103 if (c2 == "set") {
pd0wm 0:bec310bde899 104 if (set) {
pd0wm 0:bec310bde899 105 sscanf (c3.c_str(),"%d",&n);
pd0wm 0:bec310bde899 106 zeil->set(n);
pd0wm 0:bec310bde899 107 }
pd0wm 0:bec310bde899 108 } else if (c2 == "get") {
pd0wm 0:bec310bde899 109 if (!set) {
pd0wm 0:bec310bde899 110 zender.printf("%d\n\r",zeil->get());
pd0wm 0:bec310bde899 111 }
pd0wm 0:bec310bde899 112 }
pd0wm 0:bec310bde899 113
pd0wm 0:bec310bde899 114 else if (c2 == "auto") {
pd0wm 0:bec310bde899 115 if (set) {
pd0wm 0:bec310bde899 116 sscanf (c3.c_str(),"%d",&n);
pd0wm 0:bec310bde899 117 if (n == 1) {
pd0wm 0:bec310bde899 118 zeiltimer.attach(this,&Status::autoZeil,0.1);
pd0wm 0:bec310bde899 119 zeilmanual = 0;
pd0wm 0:bec310bde899 120 }
pd0wm 0:bec310bde899 121 if (n == 0) {
pd0wm 0:bec310bde899 122 zeiltimer.detach();
pd0wm 0:bec310bde899 123 zeilmanual = 1;
pd0wm 0:bec310bde899 124
pd0wm 0:bec310bde899 125 }
pd0wm 0:bec310bde899 126 }
pd0wm 0:bec310bde899 127 if (!set) {
pd0wm 0:bec310bde899 128 zender.printf("%d\n\r",zeilmanual);
pd0wm 0:bec310bde899 129
pd0wm 0:bec310bde899 130 }
pd0wm 0:bec310bde899 131 }
pd0wm 0:bec310bde899 132 } else if (c1 == "pid") {
pd0wm 0:bec310bde899 133 if (c2 == "enable") {
pd0wm 0:bec310bde899 134 if (set) {
pd0wm 0:bec310bde899 135 sscanf (c3.c_str(),"%d",&n);
pd0wm 0:bec310bde899 136 if (n == 1) {
pd0wm 0:bec310bde899 137 timer.attach(pid,&Pid::update,0.1);
pd0wm 0:bec310bde899 138 manual = 0;
pd0wm 0:bec310bde899 139 }
pd0wm 0:bec310bde899 140 if (n == 0) {
pd0wm 0:bec310bde899 141 timer.detach();
pd0wm 0:bec310bde899 142 manual = 1;
pd0wm 0:bec310bde899 143
pd0wm 0:bec310bde899 144 }
pd0wm 0:bec310bde899 145 }
pd0wm 0:bec310bde899 146 if (!set) {
pd0wm 0:bec310bde899 147 zender.printf("%d\n\r",manual);
pd0wm 0:bec310bde899 148
pd0wm 0:bec310bde899 149 }
pd0wm 0:bec310bde899 150
pd0wm 0:bec310bde899 151 } else if (c2 == "info") {
pd0wm 0:bec310bde899 152 if (!set) {
pd0wm 0:bec310bde899 153 zender.printf("%d %f %f %f %f %f %f %f %f %f %d %d %d\n\r",manual,pid->getKP(),pid->getKI(),pid->getKD(),pid->getP(),pid->getI(),pid->getD(),pid->getE(),pid->getSP(),pid->getPV(),pid->getO(),route->getLoopMode(),got->getminhoek());
pd0wm 0:bec310bde899 154
pd0wm 0:bec310bde899 155 }
pd0wm 0:bec310bde899 156
pd0wm 0:bec310bde899 157 } else if (c2 == "setp") {
pd0wm 0:bec310bde899 158 if (set) {
pd0wm 0:bec310bde899 159 sscanf (c3.c_str(),"%f",&f);
pd0wm 0:bec310bde899 160 pid->setKp(f);
pd0wm 0:bec310bde899 161
pd0wm 0:bec310bde899 162
pd0wm 0:bec310bde899 163 }
pd0wm 0:bec310bde899 164 } else if (c2 == "seti") {
pd0wm 0:bec310bde899 165 if (set) {
pd0wm 0:bec310bde899 166 sscanf (c3.c_str(),"%f",&f);
pd0wm 0:bec310bde899 167 pid->setKi(f);
pd0wm 0:bec310bde899 168
pd0wm 0:bec310bde899 169
pd0wm 0:bec310bde899 170 }
pd0wm 0:bec310bde899 171 } else if (c2 == "setd") {
pd0wm 0:bec310bde899 172 if (set) {
pd0wm 0:bec310bde899 173 sscanf (c3.c_str(),"%f",&f);
pd0wm 0:bec310bde899 174 pid->setKd(f);
pd0wm 0:bec310bde899 175
pd0wm 0:bec310bde899 176
pd0wm 0:bec310bde899 177 }
pd0wm 0:bec310bde899 178
pd0wm 0:bec310bde899 179 } else if (c2 == "manual") {
pd0wm 0:bec310bde899 180 if (set) {
pd0wm 0:bec310bde899 181 sscanf (c3.c_str(),"%d",&n);
pd0wm 0:bec310bde899 182 int m = n;
pd0wm 0:bec310bde899 183 sscanf (c4.c_str(),"%d",&n);
pd0wm 0:bec310bde899 184
pd0wm 0:bec310bde899 185 got->manual(m,n);
pd0wm 0:bec310bde899 186
pd0wm 0:bec310bde899 187
pd0wm 0:bec310bde899 188 }
pd0wm 0:bec310bde899 189 }
pd0wm 0:bec310bde899 190
pd0wm 0:bec310bde899 191 } else if (c1 == "kompas") {
pd0wm 0:bec310bde899 192 if (c2 == "get") {
pd0wm 0:bec310bde899 193 if (!set) {
pd0wm 0:bec310bde899 194 zender.printf("%d\n\r",kompas->get());
pd0wm 0:bec310bde899 195 }
pd0wm 0:bec310bde899 196 }
pd0wm 0:bec310bde899 197 if (c2 == "cal") {
pd0wm 0:bec310bde899 198 if (set) {
pd0wm 0:bec310bde899 199 sscanf (c3.c_str(),"%d",&n);
pd0wm 0:bec310bde899 200 if (n) {
pd0wm 0:bec310bde899 201 kompas->startcal();
pd0wm 0:bec310bde899 202 } else {
pd0wm 0:bec310bde899 203 kompas->stopcal();
pd0wm 0:bec310bde899 204 }
pd0wm 0:bec310bde899 205 }
pd0wm 0:bec310bde899 206 }
pd0wm 0:bec310bde899 207 } else if (c1 == "vaantje") {
pd0wm 0:bec310bde899 208 if (c2 == "set") {
pd0wm 0:bec310bde899 209 if (set) {
pd0wm 0:bec310bde899 210 sscanf (c3.c_str(),"%d",&n);
pd0wm 0:bec310bde899 211 vaantje->set(n);
pd0wm 0:bec310bde899 212 }
pd0wm 0:bec310bde899 213 }
pd0wm 0:bec310bde899 214 } else if (c1 == "gps") {
pd0wm 0:bec310bde899 215 if (c2 == "gga") {
pd0wm 0:bec310bde899 216 if (!set) {
pd0wm 0:bec310bde899 217 zender.printf("%s",gps->getgga());
pd0wm 0:bec310bde899 218 }
pd0wm 0:bec310bde899 219 } else if (c2 == "lat") {
pd0wm 0:bec310bde899 220 if (!set) {
pd0wm 0:bec310bde899 221 zender.printf("%f",gps->get().lat);
pd0wm 0:bec310bde899 222 }
pd0wm 0:bec310bde899 223 } else if (c2 == "lon") {
pd0wm 0:bec310bde899 224 if (!set) {
pd0wm 0:bec310bde899 225 zender.printf("%f",gps->get().lon);
pd0wm 0:bec310bde899 226 }
pd0wm 0:bec310bde899 227 }
pd0wm 0:bec310bde899 228 } else if (c1 == "route") {
pd0wm 0:bec310bde899 229 if (c2 == "list") {
pd0wm 0:bec310bde899 230 if (!set) {
pd0wm 0:bec310bde899 231 for (int x = 0; x!=route->getlen(); x++) {
pd0wm 0:bec310bde899 232 zender.printf("%d) %f %f\r\n",x,route->read(x).lat,route->read(x).lon);
pd0wm 0:bec310bde899 233 }
pd0wm 0:bec310bde899 234 zender.printf("OK\r\n");
pd0wm 0:bec310bde899 235
pd0wm 0:bec310bde899 236 }
pd0wm 0:bec310bde899 237 } else if (c2 == "add") {
pd0wm 0:bec310bde899 238 if (set) {
pd0wm 0:bec310bde899 239 float la;
pd0wm 0:bec310bde899 240 float lo;
pd0wm 0:bec310bde899 241
pd0wm 0:bec310bde899 242
pd0wm 0:bec310bde899 243 sscanf(c3.c_str(),"%f", &la);
pd0wm 0:bec310bde899 244
pd0wm 0:bec310bde899 245 sscanf(c4.c_str(),"%f", &lo);
pd0wm 0:bec310bde899 246
pd0wm 0:bec310bde899 247
pd0wm 0:bec310bde899 248 pos t;
pd0wm 0:bec310bde899 249 t.lat = la;
pd0wm 0:bec310bde899 250 t.lon = lo;
pd0wm 0:bec310bde899 251
pd0wm 0:bec310bde899 252 route->add(t);
pd0wm 0:bec310bde899 253 }
pd0wm 0:bec310bde899 254 } else if (c2 == "del") {
pd0wm 0:bec310bde899 255 if (set) {
pd0wm 0:bec310bde899 256 sscanf(c3.c_str(),"%d", &n);
pd0wm 0:bec310bde899 257 route->del(n);
pd0wm 0:bec310bde899 258
pd0wm 0:bec310bde899 259 }
pd0wm 0:bec310bde899 260
pd0wm 0:bec310bde899 261 } else if (c2 == "up") {
pd0wm 0:bec310bde899 262 if (set) {
pd0wm 0:bec310bde899 263 sscanf(c3.c_str(),"%d", &n);
pd0wm 0:bec310bde899 264 route->up(n);
pd0wm 0:bec310bde899 265
pd0wm 0:bec310bde899 266 }
pd0wm 0:bec310bde899 267
pd0wm 0:bec310bde899 268 } else if (c2 == "down") {
pd0wm 0:bec310bde899 269 if (set) {
pd0wm 0:bec310bde899 270 sscanf(c3.c_str(),"%d", &n);
pd0wm 0:bec310bde899 271 route->down(n);
pd0wm 0:bec310bde899 272
pd0wm 0:bec310bde899 273 }
pd0wm 0:bec310bde899 274
pd0wm 0:bec310bde899 275 } else if (c2 == "act") {
pd0wm 0:bec310bde899 276 if (set) {
pd0wm 0:bec310bde899 277 sscanf(c3.c_str(),"%d", &n);
pd0wm 0:bec310bde899 278 route->setActive(n);
pd0wm 0:bec310bde899 279
pd0wm 0:bec310bde899 280 }
pd0wm 0:bec310bde899 281 if (!set) {
pd0wm 0:bec310bde899 282 zender.printf("%d\r\n",route->getActive());
pd0wm 0:bec310bde899 283
pd0wm 0:bec310bde899 284 }
pd0wm 0:bec310bde899 285
pd0wm 0:bec310bde899 286 } else if (c2 == "dist") {
pd0wm 0:bec310bde899 287 if (!set) {
pd0wm 0:bec310bde899 288 zender.printf("%f\r\n",route->afstand_tot_doel());
pd0wm 0:bec310bde899 289
pd0wm 0:bec310bde899 290 }
pd0wm 0:bec310bde899 291 } else if (c2 == "koers") {
pd0wm 0:bec310bde899 292 if (!set) {
pd0wm 0:bec310bde899 293 zender.printf("%f\r\n",route->koers_tot_doel());
pd0wm 0:bec310bde899 294
pd0wm 0:bec310bde899 295 }
pd0wm 0:bec310bde899 296
pd0wm 0:bec310bde899 297 } else if (c2 == "minhoek") {
pd0wm 0:bec310bde899 298 if (set) {
pd0wm 0:bec310bde899 299 sscanf(c3.c_str(),"%d", &n);
pd0wm 0:bec310bde899 300 got->setminhoek(n);
pd0wm 0:bec310bde899 301
pd0wm 0:bec310bde899 302 }
pd0wm 0:bec310bde899 303 } else if (c2 == "hys") {
pd0wm 0:bec310bde899 304 if (set) {
pd0wm 0:bec310bde899 305 sscanf(c3.c_str(),"%d", &n);
pd0wm 0:bec310bde899 306 got->sethys(n);
pd0wm 0:bec310bde899 307
pd0wm 0:bec310bde899 308 }
pd0wm 0:bec310bde899 309 } else if (c2 == "loop") {
pd0wm 0:bec310bde899 310 if (set) {
pd0wm 0:bec310bde899 311 sscanf(c3.c_str(),"%d", &n);
pd0wm 0:bec310bde899 312 route->loopMode(n);
pd0wm 0:bec310bde899 313
pd0wm 0:bec310bde899 314 }
pd0wm 0:bec310bde899 315
pd0wm 0:bec310bde899 316 }
pd0wm 0:bec310bde899 317
pd0wm 0:bec310bde899 318
pd0wm 0:bec310bde899 319
pd0wm 0:bec310bde899 320
pd0wm 0:bec310bde899 321
pd0wm 0:bec310bde899 322 }
pd0wm 0:bec310bde899 323
pd0wm 0:bec310bde899 324 }
pd0wm 0:bec310bde899 325
pd0wm 0:bec310bde899 326 }
pd0wm 0:bec310bde899 327
pd0wm 0:bec310bde899 328
pd0wm 0:bec310bde899 329 }