Torque-Limited Servo Reversal

#include <Arduino.h>
#include <U8g2lib.h>
#include <Arduino_GFX_Library.h>
#include <math.h>
#include <TAMC_GT911.h>
#include <Preferences.h>

constexpr int BACKLIGHT_PIN = 2;
constexpr int TOUCH_SDA = 19;
constexpr int TOUCH_SCL = 20;
constexpr int TOUCH_INT = 18;
constexpr int TOUCH_RST = 38;
constexpr uint16_t SCREEN_WIDTH = 800;
constexpr uint16_t SCREEN_HEIGHT = 480;
constexpr uint16_t BOX_WIDTH = 180;
constexpr uint16_t BOX_HEIGHT = 110;
constexpr uint16_t BOX_MARGIN = 18;
constexpr int LCD_DE = 40;
constexpr int LCD_VSYNC = 41;
constexpr int LCD_HSYNC = 39;
constexpr int LCD_PCLK = 42;
constexpr int LCD_R0 = 45;
constexpr int LCD_R1 = 48;
constexpr int LCD_R2 = 47;
constexpr int LCD_R3 = 21;
constexpr int LCD_R4 = 14;
constexpr int LCD_G0 = 5;
constexpr int LCD_G1 = 6;
constexpr int LCD_G2 = 7;
constexpr int LCD_G3 = 15;
constexpr int LCD_G4 = 16;
constexpr int LCD_G5 = 4;
constexpr int LCD_B0 = 8;
constexpr int LCD_B1 = 3;
constexpr int LCD_B2 = 46;
constexpr int LCD_B3 = 9;
constexpr int LCD_B4 = 1;

Arduino_ESP32RGBPanel *rgbPanel = new Arduino_ESP32RGBPanel(
  LCD_DE, LCD_VSYNC, LCD_HSYNC, LCD_PCLK,
  LCD_R0, LCD_R1, LCD_R2, LCD_R3, LCD_R4,
  LCD_G0, LCD_G1, LCD_G2, LCD_G3, LCD_G4, LCD_G5,
  LCD_B0, LCD_B1, LCD_B2, LCD_B3, LCD_B4,
  0, 8, 4, 8,
  0, 8, 4, 8,
  1, 16000000
);

Arduino_RGB_Display *display = new Arduino_RGB_Display(
  SCREEN_WIDTH, SCREEN_HEIGHT, rgbPanel, 0, false
);
Arduino_Canvas *gfx = new Arduino_Canvas(SCREEN_WIDTH, SCREEN_HEIGHT, display);


// Passive UART capture only. No bus.write(), requests, tare or motor control.
// Keep explicit RX44/TX43; never select default pins or GPIO17.
HardwareSerial bus(1);
const uint32_t formats[]={SERIAL_8N1,SERIAL_8E1};
const char *names[]={"8N1","8E1"};
Preferences settings;
uint32_t lastSummary=0;
uint8_t rx[4096];
uint32_t frameBytes=0;
size_t count=0;
bool overflow=false;
uint8_t mode=0;
uint32_t lastByte=0,firstByte=0,modeStart=0,lastDraw=0;
uint32_t rxBytes=0,frames=0,crcOK=0,overflows=0;
String status="LISTENING",frame="--",previous="";
String summary="Waiting for first 10s summary";
uint32_t summaryNumber=0;
String recent[4];
uint32_t recentNumber=0;
uint16_t crc16(const uint8_t *p,size_t n){
 uint16_t c=0xffff;
 while(n--){c^=*p++;for(int i=0;i<8;i++)c=(c&1)?(c>>1)^0xa001:c>>1;}
 return c;
}
uint32_t gapUs(){return mode==0?3646:4011;} // ceil(3.5 * 10/11 bits / 9600)
void finishFrame(bool boundary=false){
 if(!count)return;
 frames++;
 bool valid=!overflow&&!boundary&&count>=4&&crc16(rx,count)==0;
 if(valid)crcOK++;
 if(overflow)overflows++;
 status=boundary?"PARTIAL AT RESTART":overflow?"BUFFER OVERFLOW":valid?"CRC MATCH - NOT TORQUE":"CRC NOT MATCHED";
 frame="";
 for(size_t i=0;i<count&&i<18;i++){char b[4];snprintf(b,sizeof(b),"%02X ",rx[i]);frame+=b;}
 for(int i=3;i>0;i--)recent[i]=recent[i-1];
 recentNumber++;
 recent[0]="#"+String(recentNumber)+" t="+String(firstByte/1000)+"ms n="+String(frameBytes)+" "+(boundary?"PARTIAL":overflow?"OVERFLOW":valid?"CRC MATCH":"CRC ?")+" "+frame;
 count=0;frameBytes=0;overflow=false;
}
void startMode(){
 bus.begin(9600,formats[mode],44,43);
 modeStart=millis();rxBytes=frames=crcOK=overflows=0;
 status="LISTENING";frame="--";

}
void line(int y,const String &s,int size=2,uint16_t color=0xffff){
 gfx->setTextColor(color);gfx->setTextSize(size);gfx->setCursor(24,y);gfx->print(s);
}
void draw(){
 String freshness=rxBytes?(uint32_t(micros()-lastByte)>=2000000UL?"NO NEW BYTES >=2s":"RECENT BYTES"):"NO BYTES RECEIVED";
 String key=String(mode)+status+frame+String(rxBytes)+String(frames)+String(crcOK)+String(overflows)+freshness+summary;
 if(key==previous)return;previous=key;
 gfx->fillScreen(0x1083);
 line(24,"TORQUE SENSOR - LISTEN ONLY",3);
 line(78,"CYBSQ-V2 | 9600 "+String(names[mode])+" | FIXED MODE");
 line(110,"TX: 0 | RX bytes: "+String(rxBytes)+" | Groups: "+String(frames));
 line(153,status,2,crcOK?0x07e0:0xffe0);
 line(202,"CRC matches: "+String(crcOK)+" | Overflow: "+String(overflows));
 line(251,"Torque: -- N.m (not decoded)");
 line(287,freshness+" | Last HEX retained");
 for(int i=0;i<4;i++)line(325+i*24,recent[i].length()?recent[i]:"--",1);
 line(426,summary,2,0xffe0);
 line(455,"ON-SCREEN CAPTURE | USB LOG DISABLED",1,0xffe0);
 gfx->flush();display->flush();
}
void setup(){
 pinMode(BACKLIGHT_PIN,OUTPUT);digitalWrite(BACKLIGHT_PIN,HIGH);
 // USB CDC disabled: GPIO19/20 belong to touch. UART0 pins belong to RS485.
 // Do not initialize Serial or Serial0 on this wiring.
 if(!display->begin()||!gfx->begin(GFX_SKIP_OUTPUT_BEGIN)){
  digitalWrite(BACKLIGHT_PIN,LOW);while(true)delay(1000);
 }
 gfx->setTextWrap(false);
 bus.setRxBufferSize(4096);
 settings.begin("uart-test",false);
 mode=settings.getUChar("fixed",0);if(mode>1)mode=0;

 startMode();draw();lastDraw=millis();
}
void loop(){
 while(bus.available()){
  uint32_t t=micros();
  if(count&&uint32_t(t-lastByte)>=gapUs())finishFrame();
  int b=bus.read();if(b<0)break;
  if(!count)firstByte=t;
  rxBytes++;frameBytes++;if(count<sizeof(rx))rx[count++]=uint8_t(b);else overflow=true;
  lastByte=t;
 }
 if(count&&uint32_t(micros()-lastByte)>=gapUs())finishFrame();
 uint32_t now=millis();
 if(uint32_t(now-lastSummary)>=10000){
  lastSummary=now;
  summaryNumber++;
  summary="S"+String(summaryNumber)+" t="+String(now/1000)+"s RX="+String(rxBytes)+" F="+String(frames)+" CRC="+String(crcOK);

 }
 // Draw only changed content, at most twice per second and outside a frame.
 if(!count&&!bus.available()&&uint32_t(now-lastDraw)>=500){draw();lastDraw=now;}
 delayMicroseconds(100);
}