planner.cpp

00001 #include "planner.h"
00002 
00003 extern DigitalOut led1;
00004 extern DigitalOut led2;
00005 extern DigitalOut led3;
00006 extern DigitalOut led4;
00007 
00008 inline F32 dist_between(Point a, Point b) {
00009     return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z));
00010 }
00011 
00012 inline void conform_goal(Point* in) {
00013     in->x = RESTRICT(in->x, MIN_X, MAX_X);
00014     in->y = RESTRICT(in->y, MIN_Y, MAX_Y);
00015     in->z = RESTRICT(in->z, MIN_Z, MAX_Z);
00016 }
00017 
00018 Status planner_setup(Planner* planner) {
00019     // Initialize circular buffer
00020     planner->current = 0;
00021     planner->next = 0;
00022     
00023     planner->current_pos.x = 0;
00024     planner->current_pos.y = 0;
00025     planner->current_pos.z = START_Z;
00026     
00027     planner->next_pos = planner->current_pos;
00028     
00029     planner->prev_dist = 0;
00030     planner->current_angle = 500;
00031     planner->next_angle = 500;
00032     planner->last_speed = 0;
00033     
00034     return set_position(planner->current_pos);
00035 }
00036 
00037 Status reset_position(Planner* planner) {
00038     Point goal;
00039     goal.x = 0;
00040     goal.y = 0;
00041     goal.z = START_Z;
00042     return goto_point(planner, goal);
00043 }
00044 
00045 Status add_point_to_buffer(Planner* planner, Point in) {
00046     if (INC_ONE(planner->next) == planner->current)
00047         return FAILURE;
00048     planner->buffer[planner->next] = in;
00049     planner->next = INC_ONE(planner->next);
00050     return SUCCESS;
00051 }
00052 
00053 void clear_buffer(Planner* planner) {
00054     // Initial location
00055     planner->next = planner->current;
00056 }
00057 
00058 int get_num_in_buffer(Planner* planner) {
00059     if (planner->next < planner->current)
00060         return planner->next + PLANNER_BUFFER_SIZE - planner->current - 1;
00061     else
00062         return planner->next - planner->current - 1;
00063 }
00064 
00065 Status goto_point(Planner* planner, F32 x, F32 y, F32 z) {
00066     Point goal;
00067     goal.x = x;
00068     goal.y = y;
00069     goal.z = z;
00070     return goto_point(planner, goal);
00071 }
00072 
00073 Status goto_point(Planner* planner, Point goal) {
00074     Point cur  = planner->current_pos;
00075     
00076     F32 step = 0, inv_vec_mag;
00077     F32 dx, dy, dz;
00078     F32 dist, full_dist, prev_dist;
00079     
00080     Planner_State state = PLR_ACCL;
00081     
00082     conform_goal(&goal);
00083     planner->next_pos = goal;
00084 
00085     dx = goal.x - cur.x;
00086     dy = goal.y - cur.y;
00087     dz = goal.z - cur.z;
00088 
00089     dist = dist_between(cur, goal);
00090     full_dist = dist;
00091     prev_dist = dist;
00092     
00093     if (dist < MIN_STEP_SIZE)
00094         return SUCCESS;
00095 
00096     // Inverse square root!!!
00097     inv_vec_mag = 1 / dist;
00098     dx = dx * inv_vec_mag;
00099     dy = dy * inv_vec_mag;
00100     dz = dz * inv_vec_mag;
00101     
00102     // TODO: think of a better way to exit
00103     while (1) {
00104         if (state == PLR_ACCL) {
00105             if (full_dist - prev_dist > ACCL_ZONE)
00106                 state = PLR_FULL;
00107     
00108             else if (prev_dist < ACCL_ZONE && prev_dist * 2 < full_dist)
00109                 state = PLR_DECL;
00110     
00111             else
00112                 step = MAP(full_dist - prev_dist, 0, ACCL_ZONE, MIN_STEP_SIZE, MAX_STEP_SIZE);
00113         }
00114         if (state == PLR_FULL) {
00115             if (prev_dist < ACCL_ZONE)
00116                 state = PLR_DECL;
00117     
00118             else
00119                 step = MAX_STEP_SIZE;
00120         }
00121         if (state == PLR_DECL) {
00122             Point test_step;
00123             step = MAP(prev_dist, 0, ACCL_ZONE, MIN_STEP_SIZE, MAX_STEP_SIZE);
00124     
00125             test_step.x = cur.x + dx * step;
00126             test_step.y = cur.y + dy * step;
00127             test_step.z = cur.z + dz * step;
00128             
00129             dist = dist_between(test_step, goal);
00130             
00131             if (dist >= prev_dist || dist < MIN_STEP_SIZE) {
00132                 // This corrects accumulated error
00133                 cur.x = goal.x;
00134                 cur.y = goal.y;
00135                 cur.z = goal.z;
00136                 break;
00137             }
00138         }
00139         
00140         cur.x += dx * step;
00141         cur.y += dy * step;
00142         cur.z += dz * step;
00143     
00144         if (set_position(cur) != SUCCESS)
00145             return FAILURE;
00146             
00147         prev_dist = dist_between(cur, goal);
00148     }
00149     planner->current_pos = cur;
00150     return SUCCESS;
00151 }
00152 
00153 Status planner_process(Planner* planner) {
00154     if (planner->current == planner->next)
00155         return SUCCESS;
00156     if (goto_point(planner, planner->buffer[planner->current]) == SUCCESS) {
00157 
00158         if (INC_ONE(planner->current) == planner->next)
00159             return END_PAT;
00160 
00161         planner->current = INC_ONE(planner->current);
00162 
00163         return SUCCESS;
00164     }
00165     led1 = 1;
00166     return FAILURE;
00167 }
00168 
00169 Status troll_up(Planner* planner) {
00170     if (planner->current_pos.z - 0.0001 > MIN_Z) {
00171         planner->current_pos.z -= MIN_STEP_SIZE;
00172         return set_position(planner->current_pos);
00173     }
00174     return FAILURE;
00175 }
00176 
00177 Status troll_down(Planner* planner) {
00178     if (planner->current_pos.z + MIN_STEP_SIZE < MAX_Z) {
00179         planner->current_pos.z += MIN_STEP_SIZE;
00180         return set_position(planner->current_pos);
00181     }
00182     return FAILURE;
00183 }
00184 
00185 Status nudge_x(Planner* planner, F32 amount) {
00186     Point goal = planner->current_pos;
00187     goal.x += amount;
00188     conform_goal(&goal);
00189     //pc.printf("X at: %.5f delta: %.5f\n", goal.x, amount);
00190     return goto_point(planner, goal);
00191 }