/***********************************************
 * Servo teach mode for Lobot lsc32 controller  *
 * written by                                   *
 * Olaf Lampe                                   *
 * use at own risk                              *
 ************************************************/
#include <LiquidCrystal_PCF8574.h>
#include <Wire.h>
LiquidCrystal_PCF8574 lcd(0x27); // set the LCD address to 0x27 

//  the AD-converter values on esp32
unsigned int coxa_in, femur_in, tibia_in; 

//   the Servo IDs on the lsc32 controller
byte coxa_ID = 25;
byte femur_ID = 26;
byte tibia_ID = 27;

/* init message for cmd_servo_move
  header, length, command, parameter => 

 Parameter1:The number of servo to be controlled
 Parameter2:Lower 8bits of time value
 Parameter3:Higher 8bits of time value
 
 * following parameters are individual
 Parameter4:Servo ID number ( 0-32 )
 Parameter5:Lower 8bits of the angle position value ( 500-2500 )
 Parameter6:Higher 8bits of the angle position value
 */
byte init_lsc[]={0x55, 0x55, 0x08, 0x03, 0x01, 0x32, 0x00}; //  single servo move 50ms 
int len_lsc = sizeof(init_lsc);
  
void setup()
{
  Serial.begin(9600); //   connection between esp32 and lsc32 extension port (must be 9600 baud)
  Wire.begin();
  Wire.beginTransmission(0x27);
  lcd.begin(20, 4); // initialize the lcd
    lcd.home();
    lcd.clear();
    lcd.setBacklight(1);
    lcd.print("Servo teach mode");  
  
} // setup()


void loop()
{
//  Reads the potmeter values then sends them to the LCD screen and lsc32 controller to mirror the teacher-leg position  
  coxa_in = analogRead(39); //  39 = AD-converter pin on esp32
  coxa_in = map(coxa_in, 0, 4095, 500, 2500);
  lcd.setCursor(0, 1);
  lcd.print("Coxa  ");
  lcd.print(coxa_in);
  lcd.print("    ");    //clear obsolete digits
  
  Serial.write(init_lsc, len_lsc);
  Serial.write(coxa_ID);
  Serial.write(lowByte(coxa_in));
  Serial.write(highByte(coxa_in));
  //Serial.println(coxa_in);
  
  femur_in = analogRead(34);
  femur_in = map(femur_in, 0, 4095, 500, 2500);
  lcd.setCursor(0, 2);    
  lcd.print("Femur ");
  lcd.print(femur_in);
  lcd.print("    ");
  
  Serial.write(init_lsc, len_lsc);
  Serial.write(femur_ID);
  Serial.write(lowByte(femur_in));
  Serial.write(highByte(femur_in));
  //Serial.println(femur_in);
  
  tibia_in = analogRead(35); 
  tibia_in = map(tibia_in, 0, 4095, 500, 2500); 
  lcd.setCursor(0, 3);
  lcd.print("Tibia ");
  lcd.print(tibia_in);
  lcd.print("    "); 
  
  Serial.write(init_lsc, len_lsc);
  Serial.write(tibia_ID);  
  Serial.write(lowByte(tibia_in));
  Serial.write(highByte(tibia_in));
  //Serial.println(tibia_in);
  
 } // loop()