void Ascenseur::run()
{
if (in_asserv)
{
if (target == HAUT)
{
if (bumper_asc_haut.is_on())
{
in_asserv = false;
send_zeros();
}
//else if (tic_odo < TIC_HAUT)
//{
// send_monte();
//}
else
{
send_monte();
}
}
else if (target == BAS)
{
/*if (tic_odo > TIC_BAS)
{
send_desc();
}*/
if (bumper_asc_bas.is_on())
{
Serial.println("ASS FINI");
send_zeros();
in_asserv = false;
}
else
{
send_desc();
}
}
else
{
if (tic_odo > target + 10)
{
send_monte();
}
else if (tic_odo < target - 10 )
{
send_desc();
}
else if (tic_odo < target)
{
send_maintien_p();
}
else if (tic_odo >= target)
{
send_zeros();
}
}
}
}
/* send a write command to the UPS, the write command and the value to be written are passed
with a char* buffer
it retries 5 times before give up */
void send_write_command(unsigned char *command, int command_length) {
int i, retry, sent, checksum;
unsigned char raw_buf[255];
/* prepares the raw data */
raw_buf[0] = 0x02; /* STX byte */
raw_buf[1] = (unsigned char)(command_length + 1); /* data length + checksum */
memcpy(raw_buf+2, command, command_length);
command_length += 2;
/* calculate checksum */
checksum = 0;
for (i = 1; i < command_length; i++) checksum += raw_buf[i];
checksum = checksum % 256;
raw_buf[command_length] = (unsigned char)checksum;
command_length +=1;
retry = 0;
sent = 0;
while ((sent != (command_length)) && (retry < 5)) {
if (retry == 4) send_zeros(); /* last retry is preceded by a serial reset... */
sent = ser_send_buf(upsfd, raw_buf, (command_length));
if (sent != (command_length)) printf("Error sending command %d\n", raw_buf[2]);
retry += 1;
}
}
int upsdrv_initups(void)
{
upsfd = ser_open(device_path);
ser_set_speed(upsfd, device_path, B2400);
send_zeros();
return 1;
}
/* send a read command to the UPS, it retries 5 times before give up
it's a 4 byte request (STX, LENGTH, COMMAND and CHECKSUM) */
void send_read_command(char command) {
int retry, sent;
unsigned char buf[4];
retry = 0;
sent = 0;
while ((sent != 4) && (retry < 5)) {
buf[0]=0x02; /* STX Start of Transmission */
buf[1]=0x02; /* data length(data + checksum byte) */
buf[2]=command; /* command to send */
buf[3]=buf[1] + buf[2]; /* checksum (sum modulus 256 of data bytes + length) */
if (retry == 4) send_zeros(); /* last retry is preceded by a serial reset...*/
sent = ser_send_buf(upsfd, buf, 4);
retry += 1;
}
}