This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Revision 49:665bdca0f2cd, committed 2013-04-12
- Comitter:
- madcowswe
- Date:
- Fri Apr 12 21:07:00 2013 +0000
- Parent:
- 48:254b124cef02
- Child:
- 50:937e860f4621
- Commit message:
- Kalman online phase estimation works but is SOOO hacked together
Changed in this revision
--- a/Processes/AI/ai.cpp Fri Apr 12 17:03:53 2013 +0000 +++ b/Processes/AI/ai.cpp Fri Apr 12 21:07:00 2013 +0000 @@ -23,16 +23,16 @@ */ current_waypoint[0].x = 0.5; - current_waypoint[0].y = 1.85; + current_waypoint[0].y = 0.5; current_waypoint[0].theta = 0.0; current_waypoint[0].pos_threshold = 0.05; current_waypoint[0].angle_threshold = 0.05*PI; - current_waypoint[1].x = 1.2; - current_waypoint[1].y = 0.18; + current_waypoint[1].x = 2.5; + current_waypoint[1].y = 0.5; current_waypoint[1].theta = 0; - current_waypoint[1].pos_threshold = 0.01; - current_waypoint[1].angle_threshold = 0.00001; + current_waypoint[1].pos_threshold = 0.05; + current_waypoint[1].angle_threshold = 0.05*PI; current_waypoint[2].x = -999; /* @@ -52,6 +52,8 @@ secondwp->angle_threshold = 0.00001; */ motion::setNewWaypoint(current_waypoint); + + int currwptidx = 0; while(1) { Thread::wait(50); @@ -60,12 +62,8 @@ if (motion::checkWaypointStatus()) { - if ((current_waypoint+1)->x != -999) - { - motion::clearWaypointReached(); - current_waypoint++; - motion::setNewWaypoint(current_waypoint); - } + motion::setNewWaypoint(¤t_waypoint[++currwptidx % 2]); + motion::clearWaypointReached(); } motion::waypoint_flag_mutex.unlock(); }
--- a/Processes/Kalman/Kalman.cpp Fri Apr 12 17:03:53 2013 +0000 +++ b/Processes/Kalman/Kalman.cpp Fri Apr 12 21:07:00 2013 +0000 @@ -30,7 +30,7 @@ Mutex statelock; float RawReadings[maxmeasure+1]; -int sensorseenflags = 0; +volatile int sensorseenflags = 0; bool Kalman_inited = 0; @@ -109,6 +109,8 @@ frombrefoffset += constrainAngle(IR_Offsets[i] - IR_Offsets[refbeacon]); } } + + printf("Used IR info from %d beacons\r\n", cnt); X(3,0) = constrainAngle(IR_Offsets[refbeacon] + frombrefoffset/cnt); @@ -123,7 +125,7 @@ P = 0.02*0.02, 0, 0, 0, 0, 0.02*0.02, 0, 0, 0, 0, 0.4*0.4, -0.4*0.4, - 0, 0, -0.4*0.4, 0.4*0.4; + 0, 0, -0.4*0.4, 0.4*0.4 + 0.05*0.05; statelock.unlock(); @@ -143,8 +145,9 @@ void predictloop(void const*) { - OLED4 = !Printing::registerID(0, 3); - OLED4 = !Printing::registerID(1, 4); + OLED4 = !Printing::registerID(0, 3) || OLED4; + OLED4 = !Printing::registerID(1, 4) || OLED4; + OLED4 = !Printing::registerID(8, 1) || OLED4; float lastleft = 0; float lastright = 0; @@ -222,6 +225,8 @@ //Update Printing float statecpy[] = {X(0,0), X(1,0), X(2,0)}; Printing::updateval(0, statecpy, 3); + + Printing::updateval(8, X(3,0)); float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)}; Printing::updateval(1, Pcpy, 4); @@ -248,6 +253,7 @@ void runupdate(measurement_t type, float value, float variance) { sensorseenflags |= 1<<type; + RawReadings[type] = value; if (Kalman_inited) { measurmentdata* measured = (measurmentdata*)measureMQ.alloc(); @@ -364,7 +370,7 @@ } Matrix<float, 4, 1> PHt (P * trans(H)); - S = (H * PHt)(0,0) + variance; + S = (H * PHt)(0,0) + variance*10; //TODO: Temp Hack! OLED3 = 0; if (aborton2stddev && Y*Y > 4 * S) {
--- a/globals.h Fri Apr 12 17:03:53 2013 +0000 +++ b/globals.h Fri Apr 12 21:07:00 2013 +0000 @@ -13,12 +13,12 @@ const float TURRET_FWD_PLACEMENT = -0.042; //Robot movement constants -const float fwdvarperunit = 0.01; //1 std dev = 7% //TODO: measrue this!! -const float varperang = 0.0002; //around 1 degree stddev per 180 turn //TODO: measrue this!! -const float xyvarpertime = 0;//0.0005; //(very poorly) accounts for hitting things -const float angvarpertime = 0;//0.001; +const float fwdvarperunit = 0.001; //1 std dev = 7% //TODO: measrue this!! +const float varperang = 0.0005; //around 3 degree stddev per 180 turn //TODO: measrue this!! +const float xyvarpertime = 0.0001; //(very poorly) accounts for hitting things +const float angvarpertime = 0.001; -const float MOTORCONTROLLER_FILTER_K = 0.8;// TODO: tune this +const float MOTORCONTROLLER_FILTER_K = 0.5;// TODO: tune this const float MOTOR_MAX_POWER = 0.5f; /*
--- a/main.cpp Fri Apr 12 17:03:53 2013 +0000 +++ b/main.cpp Fri Apr 12 21:07:00 2013 +0000 @@ -75,8 +75,8 @@ Thread updatethread(Kalman::updateloop, NULL, osPriorityNormal, 2084); - //Ticker motorcontroltestticker; - //motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.01); + Ticker motorcontroltestticker; + motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05); // ai layer thread Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048);