void AnimationCandy::animate(Framebuffer &leds, CHSV baseColor, const uint8_t step)
{
for(uint8_t i=0; i<GARLAND_LENGTH; ++i) {
leds[garland_leds[0][i]] = CHSV(baseColor.hue, sin8(step*2), sin8(step));
leds[garland_leds[1][i]] = CHSV(baseColor.hue, sin8(step*2+127), sin8(step+127));
}
}
void AnimationMultiPoints::animate(Framebuffer &leds, CHSV baseColor, const uint8_t step)
{
Point p1(sin8(step), 42, CHSV(HUE_RED+baseColor.hue, 127, 255));
Point p2(sin8(255-step), 64, CHSV(HUE_BLUE+baseColor.hue, 255, 255));
Point p3(sin8(64-step), 32, CHSV(HUE_GREEN+baseColor.hue, 255, 255));
for(uint8_t col=0; col<COLUMNS; ++col) {
for(uint8_t line=0; line<LINES; ++line) {
const uint8_t led_id = column_leds[col][line];
CRGB color = p1.computeCHSVFor(map(realLine(col, line), 0, 2*LINES, 0, 255));
color += p2.computeCHSVFor(map(realLine(col, line), 0, 2*LINES, 0, 255));
color += p3.computeCHSVFor(map(realLine(col, line), 0, 2*LINES, 0, 255));
leds[led_id] = color;
}
}
}
void tick(unsigned long t) {
for(int col = 0; col < width; col++) {
for( int row = 0; row < height; row++) {
uint8_t s = sin8(col * 3 + t);
uint8_t c = cos8(row * 2 + t);
pixel(col, row) = qadd8(s, c);
}
}
rotate(rotation+2);
}
void motionPlan() {
// if gaze transition is in progress, ...
if(lookCount == 0) {
// if gaze transition is just starting, pick new gaze target
if(moveCount == moveLength) {
// store old positions
for(uint32_t i = 0; i < NUM_DOFS; i++)
desrOld[i] = desrNew[i];
// calculate new gaze parameters, independent from previous gaze
gazeFocus = rand()%(focusMax - focusMin) + focusMin;
gazePitch = rand()%(pitchMax - pitchMin) + pitchMin;
gazeYaw = rand()%(yawMax - yawMin) + yawMin;
// convert gaze parameters to joint positions
desrNew[0] = J00_Cnt + gazeFocus; // neck pitch
desrNew[1] = J01_Cnt + gazeYaw*1/2; // head rotation
desrNew[2] = J02_Cnt - gazeYaw*1/4; // head roll
desrNew[3] = J03_Cnt + gazePitch*2/3 + gazeFocus;
desrNew[4] = J04_Cnt - gazePitch*1/3; // eye pitch
desrNew[5] = J05_Cnt - gazeYaw*1/2; // left eye yaw
desrNew[6] = J06_Cnt - gazeYaw*1/2; // right eye yaw
desrNew[7] = eyelidsMax + 4*(gazeFocus - focusMax); // eyelids
desrNew[8] = J08_Cnt + gazePitch*2; // right ear rotation
desrNew[9] = J09_Cnt + gazeFocus*2; // right ear extension
desrNew[10] = J10_Cnt - gazePitch*2; // left ear rotation
desrNew[11] = J11_Cnt + gazeFocus*2; // left ear extension
moveCount--;
}
// if gaze transition is complete, reset counters
else if(moveCount == 0) {
// store new values directly as desired values, instead of
// interpolating
for(uint32_t i = 0; i < NUM_DOFS; i++)
desrPos[i] = desrNew[i];
// pick random time for next gaze transition
lookCount = rand()%(CTRL_FREQ*(lookPeriodMax - lookPeriodMin))
+ CTRL_FREQ*lookPeriodMin;
// reset transition counter
moveCount = moveLength;
}
// if gaze is in progress, interpolate between previous and next
else {
int32_t moveScalar = sin8(moveCount*128/moveLength + 64);
for(uint32_t i = 0; i < NUM_DOFS; i++)
desrPos[i] = desrOld[i] + (desrNew[i]-desrOld[i])*moveScalar/255;
moveCount--;
}
}
// if gaze transition is not in progress, decrement counter
else {
desrPos[7] = desrNew[7];
lookCount--;
}
// if blink is in progress, ...
if(blinkCount1 == 0) {
// if blink is completed, reset counters
if(blinkCount2 == 0) {
blinkCount1 = rand()%(CTRL_FREQ*(blinkPeriodMax - blinkPeriodMin))
+ CTRL_FREQ*blinkPeriodMin;
blinkCount2 = blinkLength;
}
else {
// if in first half of blink, interpolate from open to closed
if(blinkCount2 > blinkLength/2)
desrPos[7] = eyelidsMin + (desrPos[7] - eyelidsMin) \
*(blinkCount2 - blinkLength/2)/(blinkLength/2);
// if in second half of blink, interpolate from closed to open
else
desrPos[7] = eyelidsMin + (desrPos[7] - eyelidsMin) \
*(blinkLength/2 - blinkCount2)/(blinkLength/2);
}
blinkCount2--;
}
else
blinkCount1--;
}
