144 lines
2.6 KiB
C++
144 lines
2.6 KiB
C++
#include "adapter_ard.h"
|
|
#include "HCSR04.h"
|
|
|
|
#include <ctime>
|
|
#include <time.h>
|
|
#include <stdio.h>
|
|
#include <math.h>
|
|
#include <iostream>
|
|
|
|
serial_c Serial;
|
|
|
|
HCSR04_c HCSR04;
|
|
|
|
struct timespec start, end;
|
|
|
|
int pinState[32];
|
|
double dists[2];
|
|
|
|
int main(int argc, char** argv){
|
|
clock_gettime(CLOCK_MONOTONIC_RAW, &start);
|
|
|
|
setup();
|
|
int c = '0';
|
|
char cmd_buff[128];
|
|
int cmd_buff_ind = 0;
|
|
|
|
while(1){
|
|
while((c = getchar()) != '@'){
|
|
if(c == '$')
|
|
cmd_buff_ind = 0;
|
|
if(c == ';'){
|
|
cmd_buff[cmd_buff_ind] = '\0';
|
|
char* end_ptr;
|
|
int pin = strtol(&cmd_buff[1], &end_ptr, 10);
|
|
int level = strtol(&end_ptr[1], &end_ptr, 10);
|
|
//printf("pin %d at %d\n", pin, level);
|
|
if(pin >= 0)
|
|
pinState[pin] = level;
|
|
else dists[-pin-1] = level/10.0;
|
|
cmd_buff_ind = 0;
|
|
}
|
|
cmd_buff[cmd_buff_ind++] = c;
|
|
}
|
|
printf("$dt,%ld;", micros());
|
|
loop();
|
|
delay(10);
|
|
printf("*");
|
|
fflush(stdout);
|
|
}
|
|
}
|
|
|
|
void delay(unsigned long milli){
|
|
unsigned long end = millis() + milli;
|
|
while(millis() <= end);
|
|
}
|
|
|
|
long millis(){
|
|
clock_gettime(CLOCK_MONOTONIC_RAW, &end);
|
|
uint64_t milli = (end.tv_sec - start.tv_sec) * 1000 + (end.tv_nsec - start.tv_nsec) / 1000000;
|
|
|
|
return milli;
|
|
}
|
|
|
|
long micros(){
|
|
clock_gettime(CLOCK_MONOTONIC_RAW, &end);
|
|
uint64_t milli = (end.tv_sec - start.tv_sec) * 1000000 + (end.tv_nsec - start.tv_nsec) / 1000;
|
|
|
|
return milli;
|
|
}
|
|
|
|
int abs(int in){
|
|
return abs(in);
|
|
}
|
|
|
|
double min(double a, double b){
|
|
return a<b?a:b;
|
|
}
|
|
double max(double a, double b){
|
|
return a>b?a:b;
|
|
}
|
|
|
|
void pinMode(int pin, int mode){
|
|
printf("$pm,%d,%d;",pin, mode);
|
|
}
|
|
|
|
int analogRead(int pin){
|
|
return pinState[pin];
|
|
}
|
|
|
|
void digitalWrite(int pin, int level){
|
|
printf("$dw,%d,%d;", pin, level);
|
|
}
|
|
|
|
void analogWrite(int pin, int dc){
|
|
printf("$aw,%d,%d;", pin, dc);
|
|
}
|
|
|
|
void serial_c::begin(int br){
|
|
printf("started serial at %d bps\n", br);
|
|
}
|
|
|
|
void serial_c::print(std::string txt){
|
|
std::cout << "serial : " << txt;
|
|
}
|
|
|
|
void serial_c::println(std::string txt){
|
|
this->print(txt);
|
|
std::cout << std::endl;
|
|
}
|
|
|
|
void serial_c::print(int nb){
|
|
printf("serial : %d", nb);
|
|
}
|
|
|
|
void serial_c::print(float nb){
|
|
printf("serial : %f", nb);
|
|
}
|
|
|
|
void serial_c::println(float nb){
|
|
this->print(nb);
|
|
printf("\n");
|
|
}
|
|
void serial_c::print(double nb){
|
|
printf("serial : %f", nb);
|
|
}
|
|
|
|
void serial_c::println(double nb){
|
|
this->print(nb);
|
|
printf("\n");
|
|
}
|
|
|
|
void serial_c::println(int nb){
|
|
this->print(nb);
|
|
printf("\n");
|
|
}
|
|
|
|
void HCSR04_c::begin(byte trig, byte* echos, byte ecnt){
|
|
//printf("");
|
|
}
|
|
|
|
double* HCSR04_c::measureDistanceCm(){
|
|
//printf("dists %f %f", dists[0], dists[1]);
|
|
return dists;
|
|
} |