Diff: status/status.cpp
- Revision:
- 0:bec310bde899
diff -r 000000000000 -r bec310bde899 status/status.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/status/status.cpp Tue Sep 27 19:46:30 2011 +0000
@@ -0,0 +1,329 @@
+#include "status.h"
+
+
+Status::Status(Roer * r, Zeil * z, Kompas * k, Gps * g, Vaantje * v, Route * rout, Goto * gt, Pid * p) : zender(p13,p14) {
+ roer = r;
+ zeil = z;
+ kompas = k;
+ gps = g;
+ route = rout;
+ vaantje = v;
+ got = gt;
+ pid = p;
+
+ manual = 1;
+ zeilmanual = 1;
+
+ zender.baud(115200);
+ zender.attach(this, &Status::newData);
+}
+
+void Status::autoZeil(void) {
+ double hoek = fabs(verschil((double)kompas->get(), (double)vaantje->get()));
+ int s;
+ if (hoek <= 40)
+ s = 35;
+ else if (hoek > 40 && hoek <= 100)
+ s = (hoek - 40) * (55.0/60.0) + 35;
+ else
+ s = 90;
+ zeil->set(s);
+}
+
+void Status::newData(void) {
+
+ //buffer variables
+ char lezen;
+
+ //if data is ready in the buffer
+ while (zender.readable()) {
+ //read 1 character
+ lezen = zender.getc();
+
+ //if character is $ than it is the start of a sentence
+ if (lezen == '$') {
+ //so the pointer should be set to the first position
+ b.clear();
+
+ }
+ //write buffer character to big buffer string
+ b += lezen;
+
+ //if the character is # than the end of the sentence is reached and some stuff has to be done
+ if (lezen == '#') {
+
+ char * cstr, *p;
+ string c1,c2,c3,c4;
+ int set = 1;
+ int n;
+ float f;
+
+ //start en stop karakters afhakken
+ b.erase(0,1);
+ b.erase(b.length()-1,1);
+
+ cstr = new char [b.size()+1];
+ strcpy (cstr, b.c_str());
+
+ p = strtok(cstr," ");
+ if (p != NULL)
+ c1 = p;
+ p = strtok(NULL," ");
+ if (p != NULL)
+ c2 = p;
+ p = strtok(NULL," ");
+ if (p != NULL)
+ c3 = p;
+ else
+ set = 0;
+
+ p = strtok(NULL," ");
+ if (p != NULL)
+ c4 = p;
+
+ delete[] cstr;
+
+ if (c1 == "s") {
+ 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);
+
+ }
+
+ else if (c1 == "roer") {
+ if (c2 == "set") {
+ if (set) {
+ sscanf (c3.c_str(),"%d",&n);
+ roer->set(n);
+ }
+ } else if (c2 == "get") {
+ if (!set) {
+ zender.printf("%d\n\r",roer->get());
+ }
+ }
+ } else if (c1 == "zeil") {
+ if (c2 == "set") {
+ if (set) {
+ sscanf (c3.c_str(),"%d",&n);
+ zeil->set(n);
+ }
+ } else if (c2 == "get") {
+ if (!set) {
+ zender.printf("%d\n\r",zeil->get());
+ }
+ }
+
+ else if (c2 == "auto") {
+ if (set) {
+ sscanf (c3.c_str(),"%d",&n);
+ if (n == 1) {
+ zeiltimer.attach(this,&Status::autoZeil,0.1);
+ zeilmanual = 0;
+ }
+ if (n == 0) {
+ zeiltimer.detach();
+ zeilmanual = 1;
+
+ }
+ }
+ if (!set) {
+ zender.printf("%d\n\r",zeilmanual);
+
+ }
+ }
+ } else if (c1 == "pid") {
+ if (c2 == "enable") {
+ if (set) {
+ sscanf (c3.c_str(),"%d",&n);
+ if (n == 1) {
+ timer.attach(pid,&Pid::update,0.1);
+ manual = 0;
+ }
+ if (n == 0) {
+ timer.detach();
+ manual = 1;
+
+ }
+ }
+ if (!set) {
+ zender.printf("%d\n\r",manual);
+
+ }
+
+ } else if (c2 == "info") {
+ if (!set) {
+ 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());
+
+ }
+
+ } else if (c2 == "setp") {
+ if (set) {
+ sscanf (c3.c_str(),"%f",&f);
+ pid->setKp(f);
+
+
+ }
+ } else if (c2 == "seti") {
+ if (set) {
+ sscanf (c3.c_str(),"%f",&f);
+ pid->setKi(f);
+
+
+ }
+ } else if (c2 == "setd") {
+ if (set) {
+ sscanf (c3.c_str(),"%f",&f);
+ pid->setKd(f);
+
+
+ }
+
+ } else if (c2 == "manual") {
+ if (set) {
+ sscanf (c3.c_str(),"%d",&n);
+ int m = n;
+ sscanf (c4.c_str(),"%d",&n);
+
+ got->manual(m,n);
+
+
+ }
+ }
+
+ } else if (c1 == "kompas") {
+ if (c2 == "get") {
+ if (!set) {
+ zender.printf("%d\n\r",kompas->get());
+ }
+ }
+ if (c2 == "cal") {
+ if (set) {
+ sscanf (c3.c_str(),"%d",&n);
+ if (n) {
+ kompas->startcal();
+ } else {
+ kompas->stopcal();
+ }
+ }
+ }
+ } else if (c1 == "vaantje") {
+ if (c2 == "set") {
+ if (set) {
+ sscanf (c3.c_str(),"%d",&n);
+ vaantje->set(n);
+ }
+ }
+ } else if (c1 == "gps") {
+ if (c2 == "gga") {
+ if (!set) {
+ zender.printf("%s",gps->getgga());
+ }
+ } else if (c2 == "lat") {
+ if (!set) {
+ zender.printf("%f",gps->get().lat);
+ }
+ } else if (c2 == "lon") {
+ if (!set) {
+ zender.printf("%f",gps->get().lon);
+ }
+ }
+ } else if (c1 == "route") {
+ if (c2 == "list") {
+ if (!set) {
+ for (int x = 0; x!=route->getlen(); x++) {
+ zender.printf("%d) %f %f\r\n",x,route->read(x).lat,route->read(x).lon);
+ }
+ zender.printf("OK\r\n");
+
+ }
+ } else if (c2 == "add") {
+ if (set) {
+ float la;
+ float lo;
+
+
+ sscanf(c3.c_str(),"%f", &la);
+
+ sscanf(c4.c_str(),"%f", &lo);
+
+
+ pos t;
+ t.lat = la;
+ t.lon = lo;
+
+ route->add(t);
+ }
+ } else if (c2 == "del") {
+ if (set) {
+ sscanf(c3.c_str(),"%d", &n);
+ route->del(n);
+
+ }
+
+ } else if (c2 == "up") {
+ if (set) {
+ sscanf(c3.c_str(),"%d", &n);
+ route->up(n);
+
+ }
+
+ } else if (c2 == "down") {
+ if (set) {
+ sscanf(c3.c_str(),"%d", &n);
+ route->down(n);
+
+ }
+
+ } else if (c2 == "act") {
+ if (set) {
+ sscanf(c3.c_str(),"%d", &n);
+ route->setActive(n);
+
+ }
+ if (!set) {
+ zender.printf("%d\r\n",route->getActive());
+
+ }
+
+ } else if (c2 == "dist") {
+ if (!set) {
+ zender.printf("%f\r\n",route->afstand_tot_doel());
+
+ }
+ } else if (c2 == "koers") {
+ if (!set) {
+ zender.printf("%f\r\n",route->koers_tot_doel());
+
+ }
+
+ } else if (c2 == "minhoek") {
+ if (set) {
+ sscanf(c3.c_str(),"%d", &n);
+ got->setminhoek(n);
+
+ }
+ } else if (c2 == "hys") {
+ if (set) {
+ sscanf(c3.c_str(),"%d", &n);
+ got->sethys(n);
+
+ }
+ } else if (c2 == "loop") {
+ if (set) {
+ sscanf(c3.c_str(),"%d", &n);
+ route->loopMode(n);
+
+ }
+
+ }
+
+
+
+
+
+ }
+
+ }
+
+ }
+
+
+}
Willem Melching