//-------------------------------------------------------------------------------------------
void LCD::setBaud(byte baud)
{
//changes the baud rate.
serial.write(0x7C);
serial.write(0x07); //CTRL g
serial.write(baud); //send a value of 49 - 54
delay(100);
/*
“1” = 4800bps - 0x31 = 49
“2” = 9600bps - 0x32 = 50
“3” = 19,200bps - 0x33 = 51
“4” = 38,400bps - 0x34 = 52
“5” = 57,600bps - 0x35 = 53
“6” = 115,200bps - 0x36 = 54
*/
//these statements change the SoftwareSerial baud rate to match the baud rate of the LCD.
if(baud == 49)
{
serial.end();
serial.begin(4800);
}
if(baud == 50)
{
serial.end();
serial.begin(9600);
}
if(baud == 51)
{
serial.end();
serial.begin(19200);
}
if(baud == 52)
{
serial.end();
serial.begin(38400);
}
if(baud == 53)
{
serial.end();
serial.begin(57600);
}
if(baud == 54)
{
serial.end();
serial.begin(115200);
}
}
// Herkulex end
void HerkulexClass::end()
{
switch (port)
{
case SSerial:
SwSerial.end();
break;
#if defined (__AVR_ATmega1280__) || defined (__AVR_ATmega128__) || defined (__AVR_ATmega2560__)
case HSerial1:
Serial1.end();
break;
case HSerial2:
Serial2.end();
break;
case HSerial3:
Serial3.end();
break;
#endif
}
}
//-------------------------------------------------------------------------------------------
void LCD::restoreDefaultBaud()
{
//This function is used to restore the default baud rate in case you change it
//and forget to which rate it was changed.
serial.end();//end the transmission at whatever the current baud rate is
//cycle through every other possible buad rate and attemp to change the rate back to 115200
serial.begin(4800);
serial.write(0x7C);
serial.write(0x07);
serial.write(54);//set back to 115200
serial.end();
serial.begin(9600);
serial.write(0x7C);
serial.write(0x07);
serial.write(54);//set back to 115200
serial.end();
serial.begin(19200);
serial.write(0x7C);
serial.write(0x07);
serial.write(54);//set back to 115200
serial.end();
serial.begin(38400);
serial.write(0x7C);
serial.write(0x07);
serial.write(54);//set back to 115200
serial.end();
serial.begin(57600);
serial.write(0x7C);
serial.write(0x07);
serial.write(54);//set back to 115200
serial.end();
serial.begin(115200);
delay(10);
serial.write(0x7C);
serial.write((byte)0); //clearScreen
serial.print("Baud restored to 115200!");
delay(5000);
}
uint8_t EV3_Setup(uint8_t port, uint8_t type) // Sets up the sensor to data mode
{
if(port == PORT_1){ // PORT_1
DDRC |= 0x04; // Set the Rx pin as output
DDRC &= 0xFE;
uint8_t mode; // Calculate the mode as a number
if(type < TYPE_SENSOR_EV3_COLOR_M0)
mode = type - TYPE_SENSOR_EV3_US_M0;
else if(type < TYPE_SENSOR_EV3_GYRO_M0)
mode = type - TYPE_SENSOR_EV3_COLOR_M0;
else if(type < TYPE_SENSOR_EV3_INFRARED_M0)
mode = type - TYPE_SENSOR_EV3_GYRO_M0;
else if(type < TYPE_SENSOR_EV3_INFRARED_M5+1)
mode = type - TYPE_SENSOR_EV3_INFRARED_M0;
else mode = 0;
data16[0] = dat16[type-43];
sets[0] = setsd[type-43];
sensor1.begin(2400); // Start SoftwareSerial at base Baud rate
sensor1.write(BYTE_ACK); // Write ACK
setupDone=false;
while(!setupDone){
if(sensor1.available()){ // Check if data is available
if(sensor1.read() == BYTE_ACK){ // if an ACK Byte was read
delay(1);
sensor1.write(BYTE_ACK); // Write 2 ACKs (because, sometimes SoftwareSerial might not write the data properly)
delay(1);
sensor1.write(BYTE_ACK);
sensor1.end();
sensor1.flush();
sensor1.begin(57600); // Try to read at higher baud rate
delay(10);
if((sensor1.read() & CMD_MASK) == CMD_DATA){ // If the sensor had entered Data Mode
sensor1.write(BYTE_NACK); // Write NACK at higher baud rate
setupDone=true; // quit setup
}
else { // if corrupt data or no data was received, go back to base baud rate
sensor1.end();
sensor1.begin(2400);
}
}
}
}
sensor1.write(BYTE_NACK); // Keep the sensor in Data mode
if(mode != 0){ // If a mode other than 0 is required, write the mode datas
sensor1.write(CMD_SELECT);
sensor1.write(mode);
sensor1.write(check(0x00,CMD_SELECT,mode));
}
sensor1.write(BYTE_NACK);
}
else{ // PORT_2
DDRC |= 0x08;
DDRC &= 0xFD;
uint8_t mode;
if(type < TYPE_SENSOR_EV3_COLOR_M0)
mode = type - TYPE_SENSOR_EV3_US_M0;
else if(type < TYPE_SENSOR_EV3_GYRO_M0)
mode = type - TYPE_SENSOR_EV3_COLOR_M0;
else if(type < TYPE_SENSOR_EV3_INFRARED_M0)
mode = type - TYPE_SENSOR_EV3_GYRO_M0;
else if(type < TYPE_SENSOR_EV3_INFRARED_M5+1)
mode = type - TYPE_SENSOR_EV3_INFRARED_M0;
else mode = 0;
data16[1] = dat16[type-43];
sets[1] = setsd[type-43];
sensor2.begin(2400);
sensor2.write(BYTE_ACK);
setupDone=false;
while(!setupDone){
if(sensor2.available()){
if(sensor2.read() == BYTE_ACK){
delay(1);
sensor2.write(BYTE_ACK);
delay(1);
sensor2.write(BYTE_ACK);
sensor2.end();
sensor2.flush();
sensor2.begin(57600);
delay(10);
if((sensor2.read() & CMD_MASK) == CMD_DATA){
sensor2.write(BYTE_NACK);
setupDone=true;
}
else {
sensor2.end();
sensor2.begin(2400);
}
}
}
}
sensor2.write(BYTE_NACK);
if(mode != 0){
sensor2.write(CMD_SELECT);
sensor2.write(mode);
sensor2.write(check(0x00,CMD_SELECT,mode));
}
sensor2.write(BYTE_NACK);
}
return 0;
}
void EV3_Reset(){ // Called to reset the SoftwareSerial and use the pin for some other function
sensor1.flush();
sensor1.end();
sensor2.flush();
sensor2.end();
}
int Teleinfos::read(SoftwareSerial &cptSerial, Print &debug) {
#ifdef DEBUG
debug << F(">>getTeleinfo") << endl;
#endif
uint32_t startMillis = millis();
if ((lastRefresh !=0) && (startMillis - lastRefresh < VALID_FOR)) {
return 1;
}
cptSerial.begin(1200);
/* vider les infos de la dernière trame lue */
memset(Ligne,'\0',sizeof(Ligne));
int trameComplete=0;
reset();
while (!trameComplete) {
while(CaractereRecu != 0x02) {
if (millis()-startMillis > EDF_TIMEOUT) {
cptSerial.end();
return ERROR;
}
// boucle jusqu'a "Start Text 002" début de la trame
if (cptSerial.available()) {
CaractereRecu = cptSerial.read() & 0x7F;
}
}
while (CaractereRecu != 0x03) {
i=0;
while(CaractereRecu != 0x03 && CaractereRecu != 0x0D /* fin de ligne */) {
if (millis()-startMillis > EDF_TIMEOUT) {
cptSerial.end();
return ERROR;
}
// Tant qu'on est pas arrivé à "EndText 003" Fin de trame ou que la trame est incomplète
int available = cptSerial.available();
#ifdef DEBUG
if (available > SERIAL_BUFFER_OVERFLOW) {
debug << "O " << available << endl;
}
#endif
if (available) {
CaractereRecu = cptSerial.read() & 0x7F;
Ligne[i++]=CaractereRecu;
}
}
if (i > 0) {
Ligne[i++] = '\0';
#ifdef DEBUGLIGNE
debug << Ligne << endl;
#endif
decodeLigne(Ligne, debug);
memset(Ligne,'\0',sizeof(Ligne)); // on vide la ligne pour la lecture suivante
if (CaractereRecu == 0x0D) {
CaractereRecu = '\0';
}
}
}
// on vérifie si on a une trame complète ou non
trameComplete = isTrameComplete(debug);
#ifdef DEBUG
debug << F("complete ? ") << trameComplete << endl;
#endif
}
#ifdef DEBUG
debug << F("<<getTeleinfo") << endl;
#endif
cptSerial.end();
lastRefresh = millis();
return millis() - startMillis;
}
