Skip to content

Instantly share code, notes, and snippets.

@ddefb
Last active Oct 15, 2019
Embed
What would you like to do?
#include <AFMotor.h>
#include "controller_controller.c"
#include "car.c"
AF_DCMotor motor1(1);
AF_DCMotor motor2(2);
AF_DCMotor motor3(3);
AF_DCMotor motor4(4);
int obs_sensor;
Car__controller_mem mem;
Car__controller_out _res;
void setup() {
Serial.begin(9600);
Car__controller_reset(&mem);
pinMode(A0, OUTPUT);
pinMode(A1, INPUT);
digitalWrite(A0, LOW);
}
double read_obstacle(){
digitalWrite(A0, HIGH);
delayMicroseconds(15);
digitalWrite(A0, LOW);
uint32_t pulse_time = pulseIn(A1, HIGH);
double distance = 0.01715 * pulse_time;
return distance;
}
void loop() {
obs_sensor = read_obstacle();
Car__controller_step(obs_sensor, &_res, &mem);
motor1.setSpeed(_res.vel1);
motor1.run(_res.motor1);
motor2.setSpeed(_res.vel2);
motor2.run(_res.motor2);
motor3.setSpeed(_res.vel3);
motor3.run(_res.motor3);
motor4.setSpeed(_res.vel4);
motor4.run(_res.motor4);
delay(10);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment