#include "mbed.

h"
#include "TextLCD.h"
#include "hcsr04.h"
//#include "SevenSegmentDisplay.h"

TextLCD lcd(p15,p16,p17,p18,p19,p20); // rs, rw, e, d4, d5, d6, d7

Ticker ticker;
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led4(LED4);
//SevenSegmentDisplay segmentled( FADE );
//Serial pc(USBTX, USBRX);
//CAN can2(p9, p10);
CAN can1(p30, p29);
HCSR04 usensor(p9,p10);// trigger, echo
char counter;
unsigned int dist;
void send() {
if(can1.write(CANMessage(1337, &counter, 1))) {
// lcd.locate(0,1);
//lcd.printf("Value=%d\r\n",counter);
//dist=usensor.get_dist_cm();
led2=!led2;
}
}
void send1()
{
counter=1;
send();
}
void send2()
{
counter=2;
send();
}
int main() {
// pc.baud(115200);
//pc.printf("Applicatioin start\r\n");
can1.frequency(1000000);
while(1) {
usensor.start();

wait_ms(500);
dist=usensor.get_dist_cm();
lcd.cls();
lcd.locate(0,0);
lcd.printf("cm:%ld",dist );
if(dist<20){
led1=!led1;
send1();
}
else {
led4=!led4;
send2();
}
//can2.frequency(1000000);
//ticker.attach(&send, 1);
}
}