void gaitRunnerSetDelta(G8_RUNNER* runner, uint8_t limbNumber, DRIVE_SPEED speed ){ if(limbNumber < runner->num_actuators){ runner->delta[limbNumber] = speed; if(!gaitRunnerIsPlaying(runner)){ // Send the output now __ACTUATOR* servo = (__ACTUATOR*)pgm_read_word(&runner->actuators[limbNumber]); int16_t speed = (int16_t)(runner->speeds[limbNumber]) + (int16_t)(runner->delta[limbNumber]); speed = CLAMP(speed,DRIVE_SPEED_MIN,DRIVE_SPEED_MAX); __act_setSpeed(servo,(DRIVE_SPEED)speed); } } }
// Update the gait runner and move servos to new positions // Call it from your main loop or via the scheduler to do it in the background // NB There is no point scheduling any faster than 20ms as that is the servo refresh rate // Return true if an animation is playing boolean gaitRunnerProcess(G8_RUNNER* runner){ if(!gaitRunnerIsPlaying(runner) || runner->speeds==null){ return FALSE; } TICK_COUNT now = clockGetus(); int16_t interval = (now - runner->startTime)>>16; if(interval == 0){ return TRUE; } // There has been a noticeable change in time runner->startTime = now; if(runner->backwards){ interval *= -1; } interval *= runner->speed; // Re-check as drive speed could be zero if(interval == 0){ return TRUE; } // Locate the current animation const G8_ANIMATION* animation = &runner->animations[runner->animation]; // Update the current time with the new interval int16_t currentTime = runner->currentTime + interval; if(currentTime >= runner->totalTime){ // We have finished playing the animation if(pgm_read_byte(&animation->sweep)==FALSE){ currentTime %= runner->totalTime; // Set back to start of loop if(runner->repeatCount){ runner->repeatCount -= 1; // One less frame to go if(runner->repeatCount==0){ runner->playing = FALSE; // we have reached the end currentTime = 0; // set servos to final position } } }else{ // Start going backwards through the animation currentTime = runner->totalTime - (currentTime - runner->totalTime); runner->backwards = TRUE; } }else if(currentTime < 0){ // We have moved before the start if(pgm_read_byte(&animation->sweep)==FALSE){ currentTime = runner->totalTime + currentTime; if(runner->repeatCount){ runner->repeatCount += 1; // One more frame to go if(runner->repeatCount==0){ runner->playing = FALSE; // we have reached the end currentTime = 0; // set servos to start position } } }else{ // We have completed a sweep runner->backwards = FALSE; currentTime = -currentTime; if(runner->repeatCount){ runner->repeatCount -= 1; // One less frame to go if(runner->repeatCount==0){ runner->playing = FALSE; // we have reached the end currentTime = 0; // set servos to initial position } } } } runner->currentTime = currentTime; // range is 0....totalTime // Current time in the range 0...SCALE_X uint16_t frameTime = interpolateU(currentTime, 0,runner->totalTime, 0, SCALE_X); uint16_t frameStartTime = 0; uint16_t frameEndTime = SCALE_X; // Locate the correct frame const G8_FRAME* frame = (const G8_FRAME*)pgm_read_word(&animation->frames); uint8_t i; for(i = pgm_read_byte(&animation->numFrames)-1; i>0; i--){ const G8_FRAME* f = &frame[i]; frameStartTime = pgm_read_word(&f->time); if(frameStartTime <= frameTime){ frame = f; break; } frameEndTime = frameStartTime; frameStartTime = 0; } runner->frame = i; #ifdef DEBUG rprintf("\n%u,%d",i,currentTime); #endif // Now have:- frameStartTime <= frameTime <= frameEndTime // We now need to find the distance along the curve (0...1) that represents // the x value = frameTime // First guess from 0..1 uint16_t frameTimeOffset = frameTime-frameStartTime; double distanceGuess = ((double)(frameTimeOffset)) / ((double)(frameEndTime-frameStartTime)); const G8_LIMB_POSITION* limb = (const G8_LIMB_POSITION*)pgm_read_word(&frame->limbs); for(uint16_t l = 0; l < runner->num_actuators; l++, limb++){ double distanceMin = 0.0; double distanceMax = 1.0; double distance = distanceGuess; // Find the correct distance along the line for the required frameTime for(uint8_t iterations=0; iterations<20; iterations++){ uint16_t actualX = calcX(limb, distance); if(actualX == frameTimeOffset) break; // Found it if( actualX < frameTimeOffset){ // We need to increase t distanceMin = distance; }else{ distanceMax = distance; } // Next guess is half way between distance = distanceMin + ((distanceMax - distanceMin) / 2); } // We now know the distance runner->speeds[l] = calcY(limb,distance); #ifdef DEBUG rprintf(",%d",speed); #endif } // next limb #ifndef DEBUG // Set all the servo speeds in quick succession for(uint16_t l = 0; l < runner->num_actuators; l++){ __ACTUATOR* servo = (__ACTUATOR*)pgm_read_word(&runner->actuators[l]); int16_t speed = (int16_t)(runner->speeds[l]) + (int16_t)(runner->delta[l]); speed = CLAMP(speed,DRIVE_SPEED_MIN,DRIVE_SPEED_MAX); __act_setSpeed(servo,(DRIVE_SPEED)speed); } #endif return gaitRunnerIsPlaying(runner); }
static void setSpeed(const GAIT_DESIGNER* gait, uint8_t servo, int8_t percent){ int16_t temp = (int16_t)percent * DRIVE_SPEED_MAX; DRIVE_SPEED speed = temp / 100; __ACTUATOR* act = getEntry(gait,servo); __act_setSpeed(act,speed); }