Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
Revision 8:ffc7d8af2d5a, committed 2012-04-27
- Comitter:
- narshu
- Date:
- Fri Apr 27 18:36:54 2012 +0000
- Parent:
- 7:f9c59a3e4155
- Child:
- 9:377560539b74
- Commit message:
- UI is working
Changed in this revision
--- a/Kalman/Kalman.cpp Fri Apr 27 16:37:26 2012 +0000 +++ b/Kalman/Kalman.cpp Fri Apr 27 18:36:54 2012 +0000 @@ -167,6 +167,9 @@ void Kalman::predictloop() { + OLED4 = !ui.regid(0, 3); + OLED4 = !ui.regid(1, 9); + float lastleft = 0; float lastright = 0; @@ -234,12 +237,15 @@ 0, 0, varang + varangdt; P = F * P * trans(F) + Q; + + //Update UI + float statecpy[] = {X(0), X(1), X(2)}; + OLED4 = !ui.updateval(0, statecpy, 3) || OLED4; + + float Pcpy[] = {P(0,0), P(0,1), P(0,2), P(1,0), P(1,1), P(1,2), P(2,0), P(2,1), P(2,2)}; + OLED4 = !ui.updateval(1, Pcpy, 9) || OLED4; statelock.unlock(); - //Thread::wait(PREDICTPERIOD); - - //cout << "predict" << X << endl; - //cout << P << endl; } }
--- a/Kalman/Sonar/RFSRF05.cpp Fri Apr 27 16:37:26 2012 +0000 +++ b/Kalman/Sonar/RFSRF05.cpp Fri Apr 27 18:36:54 2012 +0000 @@ -141,7 +141,8 @@ ValidPulse = false; //Calucate distance - _dist[_beacon_counter] = _timer.read_us()/2.9 + 300; + //true offset is about 330, we put 400 so circles overlap + _dist[_beacon_counter] = _timer.read_us()/2.9 + 400; if (callbackfunc) (*callbackfunc)(_beacon_counter, _dist[_beacon_counter]);
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Fri Apr 27 18:36:54 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/libraries/Servo/le3jpb \ No newline at end of file
--- a/front_arms.h Fri Apr 27 16:37:26 2012 +0000 +++ b/front_arms.h Fri Apr 27 18:36:54 2012 +0000 @@ -1,15 +1,22 @@ #ifndef _FRONT_ARMS_H #define _FRONT_ARMS_H -#include"mbed.h" +#include "mbed.h" #define LEFT_ARM_PIN p21 #define RIGHT_ARM_PIN p24 -Ser +Servo leftServo(LEFT_ARM_PIN); +Servo rightServo(RIGHT_ARM_PIN); -void frontArmsInit() { - +void setLeftArmPosition(float degrees) { + if(degrees < 90) degrees = 90; // If arm moves past this angle, robot will not fit in the perimeter constraint + leftServo.position(degrees); +} + +void setRightArmPosition(float degrees) { + if(degrees > 90) degrees = 90; // If arm moves past this angle, robot will not fit in the perimeter constraint + rightServo.position(degrees); } #endif // _FRONT_ARMS_H \ No newline at end of file
--- a/ui/ui.cpp Fri Apr 27 16:37:26 2012 +0000 +++ b/ui/ui.cpp Fri Apr 27 18:36:54 2012 +0000 @@ -4,7 +4,7 @@ #include "system.h" UI::UI() : - tUI(printtw,this,osPriorityNormal,1024) { + tUI(printtw,this,osPriorityNormal,2048) { newdataflags = 0; for (int i = 0; i < NUMIDS; i++) { idlist[i] = 0; @@ -25,15 +25,16 @@ } bool UI::updateval(char id, float* buffer, unsigned int length) { - + //check if the id is registered, and has buffer of correct length if (id < NUMIDS && idlist[id] == length && buffarr[id] && !(newdataflags & (1<<id))) { for (int i = 0; i < length; i++) buffarr[id][i] = buffer[i]; newdataflags |= (1<<id); return true; - } else + } else{ return false; + } } bool UI::updateval(char id, float value) { @@ -60,28 +61,36 @@ void UI::printloop() { - OLED4 = !OLED4; Thread::wait(1500); char* sync = "ABCD"; std::cout.write(sync, 4); - std::cout.flush(); - //std::cout << std::endl; + //std::cout.flush(); + std::cout << std::endl; //printf("\r\n"); - - OLED4 = !OLED4; while (1) { + + OLED3 = !OLED3; + + //send number of packets + char numtosend = 0; + for (int id = 0; id < NUMIDS; id++) + if (newdataflags & (1<<id)) + numtosend++; + + std::cout.put(numtosend); //send packets - for (int id = 0; id < NUMIDS; id++) { + for (char id = 0; id < NUMIDS; id++) { if (newdataflags & (1<<id)) { std::cout.put(id); - std::cout.write((char*)buffarr[id], 4*idlist[id]); + std::cout.write((char*)buffarr[id], idlist[id] * sizeof(float)); newdataflags &= ~(1<<id); } } - + + std::cout << std::endl; //std::cout.flush(); Thread::wait(200); }
--- a/ui/ui.h Fri Apr 27 16:37:26 2012 +0000 +++ b/ui/ui.h Fri Apr 27 18:36:54 2012 +0000 @@ -18,9 +18,10 @@ bool unregid(char id); private: + Mutex printlock; char idlist[NUMIDS]; float* buffarr[NUMIDS]; - int newdataflags; //Only works for NUMID = 32 + volatile int newdataflags; //Only works for NUMID = 32 void printloop(); static void printtw(void const *arg){ ((UI*)arg)->printloop(); }