forked from MDL29/JacoBot
64 lines
1.6 KiB
C++
64 lines
1.6 KiB
C++
// Made by Linux_Hat
|
|
#include <Arduino.h>
|
|
#include "Radio.h"
|
|
#include "MotorEncoder.h"
|
|
#include "Car.h"
|
|
|
|
#define MLEFT_ENC_PIN_A 3
|
|
#define MLEFT_ENC_PIN_B A4
|
|
#define MLEFT_E_MOTOR 5
|
|
#define MLEFT_M_MOTOR 4
|
|
#define MLEFT_ORIENTATION false
|
|
#define MLEFT_RATIO 120
|
|
#define MLEFT_RADIUS 3.3
|
|
|
|
#define MRIGHT_ENC_PIN_A 2
|
|
#define MRIGHT_ENC_PIN_B A4
|
|
#define MRIGHT_E_MOTOR 5
|
|
#define MRIGHT_M_MOTOR 4
|
|
#define MRIGHT_ORIENTATION false
|
|
#define MRIGHT_RATIO 120
|
|
#define MRIGHT_RADIUS 3.3
|
|
|
|
#define LED_ORANGE 12
|
|
|
|
#define SIZE 500
|
|
|
|
MotorEncoder m1(MLEFT_ENC_PIN_A, MLEFT_ENC_PIN_B, MLEFT_E_MOTOR, MLEFT_M_MOTOR, MLEFT_ORIENTATION, MLEFT_RATIO, MLEFT_RADIUS); // Initialise motor
|
|
MotorEncoder m2(MRIGHT_ENC_PIN_A, MRIGHT_ENC_PIN_B, MRIGHT_E_MOTOR, MRIGHT_M_MOTOR, MRIGHT_ORIENTATION, MRIGHT_RATIO, MRIGHT_RADIUS); // Initialise motor with pins
|
|
|
|
Car car(&m1, &m2, 110);
|
|
|
|
float m1_duration = 0;
|
|
float m2_duration = 0;
|
|
|
|
void encoder_handler_m1() {
|
|
m1.wheelSpeed(); // Call wheelSpeed() of the m1 MotorEncoder instance
|
|
}
|
|
|
|
void encoder_handler_m2() {
|
|
m2.wheelSpeed(); // Call wheelSpeed() of the m2 MotorEncoder instance
|
|
}
|
|
|
|
Radio* radio;
|
|
|
|
void setup() {
|
|
Serial.begin(9600);
|
|
pinMode(LED_ORANGE, OUTPUT);
|
|
radio = new Radio(120, 9, 7);
|
|
m1.EncoderHandlerInit(encoder_handler_m1);
|
|
m2.EncoderHandlerInit(encoder_handler_m2);
|
|
Serial.println("Car started");
|
|
}
|
|
|
|
void loop() {
|
|
Serial.println("Forward 50 cm");
|
|
car.forward(SIZE,50);
|
|
Serial.println("Turn on the right");
|
|
car.turn(90, 20);
|
|
if (radio->read() && strcmp(radio->message, "STOP") == 0){
|
|
Serial.println("Stop car");
|
|
return;
|
|
|
|
}
|
|
} |