Omnirobot

def bounded_effort(values,limits=DEFAULT_LIMITS):
    if not isinstance(values,(list,tuple)) or len(values)!=3 or not all(math.isfinite(v) for v in values):
        raise ValueError('Invalid visual effort')
    forward,lateral,turn=values
    if forward<0 or lateral!=0:raise ValueError('Visual control supports forward and turn only')
    forward=min(forward,MAX_FORWARD_EFFORT*limits[0]/DEFAULT_LIMITS[0])
    turn=max(-MAX_TURN_EFFORT*limits[2]/DEFAULT_LIMITS[2],min(MAX_TURN_EFFORT*limits[2]/DEFAULT_LIMITS[2],turn))
    scale=max(1.,(forward+abs(turn))/MAX_COMBINED_EFFORT)
    return float(forward/scale),0.,float(turn/scale)


def visual_frame_time(frame,evidence,now):
    if evidence.get('qualified_frame') is not True or evidence.get('source')!=CONTROL_MODE:
        raise ValueError('; '.join(evidence.get('blockers',[])) or 'Waiting for camera/IMU timing')
    if (frame.boot,frame.session,frame.profile)!=(evidence.get('boot'),evidence.get('session'),evidence.get('profile')):
        raise ValueError('Visual frame identity changed')
    delta=(frame.captured_us-evidence['captured_us'])/1e6
    stamp=evidence['captured_monotonic']+delta
    if not -.1<=delta<=.25 or not 0<=now-stamp<=.25:
        raise ValueError('Visual capture expired')
    return stamp

// Each independent axis is a signed fraction of full PWM. Normalize combined
// wheel demand only if it exceeds one, preserving translation/rotation ratios.
inline void mix_effort(float forward,float lateral,float rotation,float out[4]) {
  const float raw[4]={forward-lateral+rotation,forward+lateral-rotation,
                      forward+lateral+rotation,forward-lateral-rotation};
  const int order[4]={0,2,3,1};
  float scale=1.f;
  for(float value:raw) scale=std::max(scale,std::fabs(value));
  for(int i=0;i<4;i++) out[i]=-raw[order[i]]/scale*255.f;
}

    def snapshot(self, now):
        if not _finite(now):
            raise ValueError('Finite execution timestamp required')
        age = None if self.stamp is None else now-self.stamp
        fresh = age is not None and 0 <= age <= MAX_COMMAND_GAP_S+1e-9
        transmitting = self.transmitting and fresh
        reason = self.reason
        if self.transmitting and not fresh:
            reason = 'Controller clock changed' if age is not None and age < 0 else 'Controller updates expired'
        pwm = dict(status='Unavailable', fresh=False, age_ms=None, channels=[])
        if self.pwm is not None and self.pwm_observed_at is not None:
            elapsed = (now-self.pwm_observed_at)*1000
            if elapsed >= 0:
                pwm_age = self.pwm['age_ms']+elapsed
                pwm.update(age_ms=pwm_age, status='Stale')
                if self.pwm.get('qualified') and pwm_age <= MAX_PWM_AGE_MS+1e-6:
                    pwm.update(age_ms=pwm_age, status='Live', fresh=True,
                               source=self.pwm['source'], mapping_verified=self.pwm['mapping_verified'],
                               channels=[dict(value) for value in self.pwm['channels']])
        return dict(stamp_monotonic=self.stamp,
                    update_age_ms=age*1000 if age is not None and age >= 0 else None,
                    commanded_nonzero_s=round(self.total_s, 6),
                    continuous_commanded_s=round(self.continuous_s, 6) if transmitting else 0.,
                    sent_commands=self.sent_commands, nonzero_commands_sent=self.nonzero_commands,
                    interruptions=self.interruptions,
                    requested=list(self.requested) if self.requested is not None else None,
                    sent=list(self.sent) if self.sent is not None else None,
                    withholding_reason=reason, transmitting_nonzero=bool(transmitting),
                    active=self.active, shadow=self.shadow, pwm=pwm,
                    evidence_limit=EVIDENCE_LIMIT)