Odradek Motion Light Controller

#include <Arduino.h>
#include <Wire.h>
#include <math.h>
// All angles are installation-specific examples, NOT measured mechanism limits.
// Test unloaded, align horns at the folded pose, then type arm. No boot pulses.
namespace Config {
const uint8_t PCA = 0x40;
const uint8_t enablePin=2; // PCA OE active LOW
const uint32_t OSC_HZ = 25000000UL; // Measure/calibrate your PCA oscillator if needed.
const uint8_t PRESCALE = 121; // nominal 50 Hz, no Servo library/timer conflict
const float closed[5] = {90,90,90,90,90};
const float active[5] = {100,100,100,100,100};
const float opened[5] = {140,140,140,140,140};
const float armMin[5] = {70,70,70,70,70};
const float armMax[5] = {150,150,150,150,150};
const uint16_t armPulseMin[5] = {1000,1000,1000,1000,1000};
const uint16_t armPulseMax[5] = {2000,2000,2000,2000,2000};
// These pulses map to 0..180 logical degrees. Verify actual arm servo endpoints.
// Head: actual POSITIONAL servo's angle between calibrated pulse endpoints.
// A normal 180-degree servo stays 180 here. Genuine 270 variant: set span=270
// and its VERIFIED pulses (not automatically 500..2500), then safe endpoints.
const float headServoSpan = 180;
const uint16_t headPulseMin = 1000, headPulseMax = 2000;
const float headSafeMin = 60, headSafeMax = 120, headHome = 90;
// Head sweep is always bounded to <=270 degrees, even if config is mistaken.
const bool headScanMode2=false, headScanMode3=true, headScanClick=true;
const bool alertArmMotion=false;
const float poseSpeed=22, clickSpeed=40, headSpeed=8; // degrees per second
const float clickTravel=1.0f; // 0..1 of active->open distance
const uint16_t clickPauseMs=300, armStaggerMs=65, alertHalfPeriodMs=850;
const float alertTravel=3;
const uint16_t servoTickMs=20, lightTickMs=10;
// Channels: strip R,G,B; independent blue LED; independent white driver.
const uint8_t lightPins[5]={3,5,6,9,10};
struct Light { uint8_t r,g,b,stripBrightness,blue,white; };
const Light lights[5]={
 {165,220,255,255,255,240}, // mode 1 icy
 {100,180,255,180,170,150}, // mode 2 deeper blue
 {175,225,255,255,255,255}, // mode 3
 {175,225,255,255,255,255}, // mode 3.5
 {255,65,0,255,0,0}        // mode 4: no white to wash out orange
};
}
enum Mode : uint8_t {IDLE,ACTIVE,SCAN,CLICK,ALERT};
Mode mode=IDLE;
float pos[6], target[6];
uint8_t lightNow[5]={}, lightTarget[5]={};
uint16_t lastPulse[6]={};
bool enabled=false, healthy=true, outward=true, headForward=true, waiting=false;
uint32_t entered=0, phaseStart=0, pauseStart=0, lastServo=0,lastLight=0;
char command[16]; uint8_t commandLen=0; bool overflow=false;
float bounded(float v,float lo,float hi){return max(lo,min(hi,v));}
float headLo(){return bounded(Config::headSafeMin,0,min(270.0f,Config::headServoSpan));}
float headHi(){return bounded(Config::headSafeMax,headLo(),min(270.0f,Config::headServoSpan));}
bool regWrite(uint8_t reg,uint8_t val){Wire.beginTransmission(Config::PCA);Wire.write(reg);Wire.write(val);return Wire.endTransmission()==0;}
bool pulseWrite(uint8_t ch,uint16_t us){
 uint16_t ticks=(uint32_t)us* (Config::OSC_HZ/1000UL) / ((uint32_t)(Config::PRESCALE+1)*1000UL);
 Wire.beginTransmission(Config::PCA);Wire.write(0x06+4*ch);
 Wire.write(0);Wire.write(0);Wire.write(ticks&255);Wire.write(ticks>>8);
 return Wire.endTransmission()==0;
}
void stopOutputs(){digitalWrite(Config::enablePin,HIGH);regWrite(0xFD,0x10);enabled=false;}
void fail(){healthy=false;stopOutputs();Serial.println(F("I2C fault: cut servo power; inspect wiring and reset."));}
void setLighting(){
 const Config::Light &l=Config::lights[mode];
 lightTarget[0]=(uint16_t)l.r*l.stripBrightness/255;
 lightTarget[1]=(uint16_t)l.g*l.stripBrightness/255;
 lightTarget[2]=(uint16_t)l.b*l.stripBrightness/255;
 lightTarget[3]=l.blue; lightTarget[4]=l.white;
}
void selectMode(Mode m){mode=m;entered=phaseStart=millis();waiting=false;outward=true;headForward=true;setLighting();Serial.println(F("Mode selected."));}
void armsPose(const float *pose){for(uint8_t i=0;i<5;i++)target[i]=pose[i];}
void headScan(bool scan){
 if(!scan){target[5]=bounded(Config::headHome,headLo(),headHi());return;}
 if(headForward && pos[5]>=headHi()-0.01f)headForward=false;
 else if(!headForward && pos[5]<=headLo()+0.01f)headForward=true;
 target[5]=headForward?headHi():headLo();
}
void mode1FoldedIdle(){armsPose(Config::closed);headScan(false);}
void mode2FoldedActive(){armsPose(Config::active);headScan(Config::headScanMode2);}
void mode3FullScan(){armsPose(Config::opened);headScan(Config::headScanMode3);}
void mode35ArticulatedClick(uint32_t now){
 bool reached=true;
 for(uint8_t i=0;i<5;i++){
  if((uint32_t)(now-phaseStart)<(uint32_t)i*Config::armStaggerMs){reached=false;continue;}
  float end=Config::active[i]+bounded(Config::clickTravel,0,1)*(Config::opened[i]-Config::active[i]);
  target[i]=bounded(outward?end:Config::active[i],Config::armMin[i],Config::armMax[i]);
  if(fabs(pos[i]-target[i])>0.01f)reached=false;
 }
 if(reached){
  if(!waiting){waiting=true;pauseStart=now;}
  if((uint32_t)(now-pauseStart)>=Config::clickPauseMs){outward=!outward;phaseStart=now;waiting=false;}
 }
 headScan(Config::headScanClick);
}
void mode4OrangeAlert(uint32_t now){
 armsPose(Config::opened);headScan(true); // bounded sweep, ALWAYS reverses
 if(Config::alertArmMotion){
  bool twitch=((now-entered)/Config::alertHalfPeriodMs)&1;
  for(uint8_t i=0;i<5;i++)if(twitch)target[i]-=Config::alertTravel;
 }
}
void updateMotion(uint32_t now){
 if(!enabled)return;
 if((uint32_t)(now-lastServo)<Config::servoTickMs)return;
 lastServo=now; // cap step after a stall; never make a catch-up jump
 switch(mode){case IDLE:mode1FoldedIdle();break;case ACTIVE:mode2FoldedActive();break;case SCAN:mode3FullScan();break;case CLICK:mode35ArticulatedClick(now);break;case ALERT:mode4OrangeAlert(now);break;}
 for(uint8_t i=0;i<6;i++){
  target[i]=i<5?bounded(target[i],Config::armMin[i],Config::armMax[i]):bounded(target[i],headLo(),headHi());
  float step=(i==5?Config::headSpeed:(mode==CLICK?Config::clickSpeed:Config::poseSpeed))*Config::servoTickMs/1000.0f;
  float delta=target[i]-pos[i];pos[i]+=bounded(delta,-step,step);
  uint16_t us=i<5 ? Config::armPulseMin[i]+pos[i]/180.0f*(Config::armPulseMax[i]-Config::armPulseMin[i]) : Config::headPulseMin+pos[i]/Config::headServoSpan*(Config::headPulseMax-Config::headPulseMin);
  if(us!=lastPulse[i]){if(!pulseWrite(i,us)){fail();return;}lastPulse[i]=us;}
 }
 // Once pose is reached no new writes. Servos retain holding pulses/torque.
}
void updateLights(uint32_t now){
 if((uint32_t)(now-lastLight)<Config::lightTickMs)return;lastLight=now;
 for(uint8_t i=0;i<5;i++){
  if(lightNow[i]==lightTarget[i])continue;
  lightNow[i]+=lightNow[i]<lightTarget[i]?1:-1;
  analogWrite(Config::lightPins[i],lightNow[i]);
 }
}
void handleCommand(){
 if(!strcmp(command,"1")||!strcmp(command,"home")){selectMode(IDLE);}
 else if(!strcmp(command,"2"))selectMode(ACTIVE);
 else if(!strcmp(command,"3"))selectMode(SCAN);
 else if(!strcmp(command,"3.5"))selectMode(CLICK);
 else if(!strcmp(command,"4"))selectMode(ALERT);
 else if(!strcmp(command,"off")){stopOutputs();Serial.println(F("Signals OFF, load can fall. Re-align folded pose before arm."));}
 else if(!strcmp(command,"arm")){
  if(enabled){selectMode(IDLE);return;}
  if(!healthy){Serial.println(F("Reset after fixing I2C."));return;}
  stopOutputs();selectMode(IDLE);
  for(uint8_t i=0;i<6;i++){pos[i]=target[i]=i<5?Config::closed[i]:bounded(Config::headHome,headLo(),headHi());lastPulse[i]=0;}
  // Seed known aligned pose with OE still HIGH, then enable all at once.
  for(uint8_t i=0;i<6;i++){
   uint16_t us=i<5 ? Config::armPulseMin[i]+pos[i]/180.0f*(Config::armPulseMax[i]-Config::armPulseMin[i]) : Config::headPulseMin+pos[i]/Config::headServoSpan*(Config::headPulseMax-Config::headPulseMin);
   if(!pulseWrite(i,us)){fail();return;}lastPulse[i]=us;
  }
  digitalWrite(Config::enablePin,LOW);
  enabled=true;Serial.println(F("Enabled: assumes physically aligned folded/home pose."));
 }else Serial.println(F("Commands: arm, 1, 2, 3, 3.5, 4, home, off"));
}
void readSerial(){
 // Bounded work per loop prevents continuous serial traffic starving motion.
 for(uint8_t n=0;n<16 && Serial.available();n++){
  char c=Serial.read();
  if(c=='\r'||c=='\n'){
   if(overflow)Serial.println(F("Command too long."));
   else if(commandLen){command[commandLen]=0;handleCommand();}
   commandLen=0;overflow=false;
  }else if(!overflow){if(commandLen<sizeof(command)-1)command[commandLen++]=c;else overflow=true;}
 }
}
void setup(){
 digitalWrite(Config::enablePin,HIGH);pinMode(Config::enablePin,OUTPUT);
 Serial.begin(115200);
 for(uint8_t i=0;i<5;i++){digitalWrite(Config::lightPins[i],LOW);pinMode(Config::lightPins[i],OUTPUT);}
 Wire.begin();Wire.setClock(100000);Wire.setWireTimeout(25000,true);
 // Direct framework Wire driver; no external libraries required. PCA MODE1 AI,
 // MODE2 non-inverted totem-pole. Sleep before prescaler, wake then restart.
 healthy=regWrite(0x00,0x30) && regWrite(0x01,0x04) && regWrite(0xFD,0x10) && regWrite(0xFE,Config::PRESCALE) && regWrite(0x00,0x20);
 delayMicroseconds(600); // oscillator startup only, not an animation delay
 healthy=healthy && regWrite(0x00,0xA0);
 if(!healthy)fail();
 setLighting();
 Serial.println(F("Odradek: outputs disabled. Calibrate unloaded; align folded/home; type arm."));
 Serial.println(F("115200 baud; newline; 1 / 2 / 3 / 3.5 / 4 / home / off"));
}
void loop(){uint32_t now=millis();readSerial();updateMotion(now);updateLights(now);}