forked from MDL29/JacoBot
[NOT WORKING] I try things but it doesn't work :(
This commit is contained in:
parent
1b886bab7d
commit
8294b9c0d1
88
jacobot/arduino/Car.cpp
Normal file
88
jacobot/arduino/Car.cpp
Normal file
|
@ -0,0 +1,88 @@
|
|||
// Made by Linux_Hat
|
||||
#include "Car.h"
|
||||
#include "MotorEncoder.h"
|
||||
#define pi 3.1415
|
||||
|
||||
Car::Car(MotorEncoder* m1, MotorEncoder* m2, float distance)
|
||||
: m_m1(m1), m_m2(m2), m_distance(distance) {
|
||||
}
|
||||
|
||||
void Car::forward(float distance, double setPoint) { //distance : mm
|
||||
const float m1_mmp = pi*(*m_m1).m_radius*2/(*m_m1).m_ratio; // mm/pulse for m1
|
||||
const float m2_mmp = pi*(*m_m2).m_radius*2/(*m_m2).m_ratio; // mm/pulse for m2
|
||||
double m1_duration = 0;
|
||||
double m2_duration = 0;
|
||||
m_m1->setPoint = setPoint;
|
||||
m_m2->setPoint = setPoint/m2_mmp*m1_mmp;
|
||||
while(m1_duration<distance/m1_mmp && m2_duration<distance/m2_mmp) {
|
||||
if(m1_duration<distance/m1_mmp) {
|
||||
m1_duration+=m_m1->duration;
|
||||
m_m1->compute(0);
|
||||
m_m1->advance();
|
||||
m_m1->duration = 0;
|
||||
} else {m_m1->stop();}
|
||||
if(m2_duration<distance/m2_mmp) {
|
||||
m2_duration+=m_m2->duration;
|
||||
m_m2->compute(0);
|
||||
m_m2->advance();
|
||||
m_m2->duration = 0;
|
||||
} else {m_m2->stop();}
|
||||
}
|
||||
m_m1->stop();
|
||||
m_m2->stop();
|
||||
}
|
||||
|
||||
void Car::turn(float angle, double setPoint) { //angle : degre
|
||||
float distance = pi*m_distance/360*angle;
|
||||
const float m1_mmp = pi*(*m_m1).m_radius*2/(*m_m1).m_ratio; // mm/pulse for m1
|
||||
const float m2_mmp = pi*(*m_m2).m_radius*2/(*m_m2).m_ratio; // mm/pulse for m2
|
||||
double m1_duration = 0;
|
||||
double m2_duration = 0;
|
||||
m_m1->setPoint = setPoint;
|
||||
m_m2->setPoint = setPoint/m2_mmp*m1_mmp;
|
||||
boolean m1_tmp = false;
|
||||
boolean m2_tmp = false;
|
||||
if(angle>0) {
|
||||
while(!m1_tmp && !m2_tmp) {
|
||||
if(m1_duration<distance/m1_mmp) {
|
||||
// Serial.print("M1 cycle ");
|
||||
// Serial.print(m1_duration);
|
||||
// Serial.print(" ");
|
||||
// Serial.println(m_m1->duration);
|
||||
m1_duration+=m_m1->duration;
|
||||
m_m1->compute(0);
|
||||
m_m1->advance();
|
||||
m_m1->duration = 0;
|
||||
} else {m1_tmp = true; m_m1->stop();}
|
||||
if(m2_duration<distance/m2_mmp) {
|
||||
// Serial.print("M2 cycle ");
|
||||
// Serial.print(m2_duration);
|
||||
// Serial.print(" ");
|
||||
// Serial.println(m_m2->duration);
|
||||
m2_duration-=m_m2->duration;
|
||||
m_m2->compute(0);
|
||||
m_m2->back();
|
||||
m_m2->duration = 0;
|
||||
} else {m2_tmp = true; m_m2->stop();}
|
||||
}
|
||||
} else if(angle<0) {
|
||||
while(!m1_tmp && !m2_tmp) {
|
||||
if(m1_duration<distance/m1_mmp) {
|
||||
m1_duration-=m_m1->duration;
|
||||
m_m1->compute(0);
|
||||
m_m1->back();
|
||||
m_m1->duration = 0;
|
||||
} else {m1_tmp = true; m_m1->stop();}
|
||||
if(m2_duration<distance/m2_mmp) {
|
||||
m2_duration+=m_m2->duration;
|
||||
m_m2->compute(0);
|
||||
m_m2->advance();
|
||||
m_m2->duration = 0;
|
||||
} else {m2_tmp = true; m_m2->stop();}
|
||||
}
|
||||
}
|
||||
Serial.println(m1_tmp);
|
||||
Serial.println("End");
|
||||
m_m1->stop();
|
||||
m_m2->stop();
|
||||
}
|
14
jacobot/arduino/Car.h
Normal file
14
jacobot/arduino/Car.h
Normal file
|
@ -0,0 +1,14 @@
|
|||
// Made by Linux_Hat
|
||||
#pragma once
|
||||
#include "MotorEncoder.h"
|
||||
|
||||
class Car {
|
||||
private:
|
||||
float m_distance;// mm
|
||||
MotorEncoder* m_m1;
|
||||
MotorEncoder* m_m2;
|
||||
public:
|
||||
Car(MotorEncoder* m1, MotorEncoder* m2, float distance);
|
||||
void forward(float distance, double setPoint);
|
||||
void turn(float angle, double setPoint);
|
||||
};
|
|
@ -1,14 +1,17 @@
|
|||
// Made by Linux_Hat
|
||||
#include <PID_v1.h>
|
||||
#include <Arduino.h>
|
||||
#include "MotorEncoder.h"
|
||||
|
||||
MotorEncoder::MotorEncoder(uint8_t encoder0pinA, uint8_t encoder0pinB, int E_left, int M_left) {
|
||||
MotorEncoder::MotorEncoder(uint8_t encoder0pinA, uint8_t encoder0pinB, int E_left, int M_left, boolean orientation, float ratio, float radius) {
|
||||
m_encoder0pinA = encoder0pinA; // pin 2 or 3
|
||||
m_encoder0pinB = encoder0pinB;
|
||||
m_E_left = E_left;// pwm pin !
|
||||
m_M_left = M_left;
|
||||
m_orientation = orientation;
|
||||
m_ratio = ratio;
|
||||
m_radius = radius;
|
||||
setPoint = 0;
|
||||
direction = true; // Default -> Forward
|
||||
pinMode(m_encoder0pinB, INPUT);
|
||||
pinMode(m_M_left, OUTPUT); // L298P Control port settings DC motor driver board for the output mode
|
||||
pinMode(m_E_left, OUTPUT);
|
||||
|
@ -23,22 +26,21 @@ void MotorEncoder::wheelSpeed() {
|
|||
int Lstate = digitalRead(m_encoder0pinA);
|
||||
if((encoder0PinALast == LOW) && Lstate==HIGH){
|
||||
int val = digitalRead(m_encoder0pinB);
|
||||
if(val == LOW && direction){
|
||||
direction = false; //Reverse
|
||||
if(val == HIGH){
|
||||
if(!m_orientation) duration--;
|
||||
else duration++;
|
||||
}
|
||||
else if(val == HIGH && !direction){
|
||||
direction = true; //Forward
|
||||
else if(val == LOW){
|
||||
if(!m_orientation) duration++;
|
||||
else duration--;
|
||||
}
|
||||
}
|
||||
encoder0PinALast = Lstate;
|
||||
|
||||
if(!direction) duration++;
|
||||
else duration--;
|
||||
|
||||
|
||||
}
|
||||
|
||||
void MotorEncoder::EncoderHandlerInit(void (*ISR)(void)) {
|
||||
direction = true; // Default -> Forward
|
||||
pinMode(m_encoder0pinB, INPUT); // Ensure encoder pin A is set as INPUT
|
||||
attachInterrupt(m_encoder0pinA-2, ISR, CHANGE);
|
||||
}
|
||||
|
@ -66,7 +68,6 @@ void MotorEncoder::compute(int debug) {
|
|||
Serial.print(duration);
|
||||
Serial.print(" Power: ");
|
||||
Serial.println(val_output);
|
||||
|
||||
}
|
||||
duration = 0;
|
||||
}
|
||||
|
|
|
@ -1,3 +1,4 @@
|
|||
// Made by Linux_Hat
|
||||
#pragma once
|
||||
#include <PID_v1.h>
|
||||
#include <Arduino.h>
|
||||
|
@ -5,22 +6,24 @@
|
|||
class MotorEncoder {
|
||||
private:
|
||||
uint8_t encoder0PinALast; //the number of the pulses
|
||||
boolean direction; //the rotation direction
|
||||
boolean m_orientation; //the rotation orientation
|
||||
boolean result;
|
||||
PID* myPID;
|
||||
double val_output; //Power supplied to the motor PWM value.
|
||||
double Kp = 0.6, Ki = 5, Kd = 0;
|
||||
double abs_duration;
|
||||
double duration;
|
||||
uint8_t m_encoder0pinA; //A pin
|
||||
uint8_t m_encoder0pinB; //B pin
|
||||
int m_E_left; //The enabling of L298PDC motor driver board connection to the digital interface port 5
|
||||
int m_M_left;
|
||||
double abs_duration;
|
||||
|
||||
public:
|
||||
double setPoint;
|
||||
float m_ratio; // how many pulse for a complete turn
|
||||
float m_radius; // wheel radius in mm
|
||||
double duration;
|
||||
|
||||
MotorEncoder(uint8_t encoder0pinA, uint8_t encoder0pinB, int E_left, int M_left);
|
||||
MotorEncoder(uint8_t encoder0pinA, uint8_t encoder0pinB, int E_left, int M_left, boolean orientation, float ratio, float radius);
|
||||
|
||||
void EncoderHandlerInit(void (*ISR)(void));
|
||||
void wheelSpeed();
|
||||
|
|
30
jacobot/arduino/Radio.cpp
Normal file
30
jacobot/arduino/Radio.cpp
Normal file
|
@ -0,0 +1,30 @@
|
|||
// Made by Linux_Hat
|
||||
#include <RF24.h>
|
||||
#include <SPI.h>
|
||||
#include <Arduino.h>
|
||||
#include "Radio.h"
|
||||
#define tunnel "PIPE1"
|
||||
|
||||
Radio::Radio(int canal, int pinCE, int pinCSN) {
|
||||
m_canal = canal;
|
||||
m_pinCE = pinCE;
|
||||
m_pinCSN = pinCSN;
|
||||
radio = new RF24(m_pinCE, m_pinCSN);
|
||||
if (!radio->begin()) {
|
||||
Serial.println("Error while initialise the NRF module");
|
||||
while (1);
|
||||
}
|
||||
Serial.println("Module initialised");
|
||||
radio->openReadingPipe(0, (uint8_t*)tunnel);
|
||||
radio->setChannel(m_canal);
|
||||
radio->setPALevel(RF24_PA_MIN);
|
||||
radio->startListening();
|
||||
}
|
||||
|
||||
char Radio::read() {
|
||||
if (radio->available()) {
|
||||
radio->read(&message, sizeof(message));
|
||||
return *message;
|
||||
}
|
||||
return 0;
|
||||
}
|
17
jacobot/arduino/Radio.h
Normal file
17
jacobot/arduino/Radio.h
Normal file
|
@ -0,0 +1,17 @@
|
|||
// Made by Linux_Hat
|
||||
#pragma once
|
||||
#include <RF24.h>
|
||||
#include <SPI.h>
|
||||
#include <Arduino.h>
|
||||
|
||||
class Radio {
|
||||
public:
|
||||
char* message;
|
||||
Radio(int canal, int pinCE, int pinCSN);
|
||||
char read();
|
||||
private:
|
||||
int m_canal;
|
||||
int m_pinCE;
|
||||
int m_pinCSN;
|
||||
RF24* radio;
|
||||
};
|
|
@ -1,33 +1,33 @@
|
|||
//The sample code for driving one way motor encoder
|
||||
// Made by Linux_Hat
|
||||
#include <Arduino.h>
|
||||
#include "radio.h"
|
||||
#include "Radio.h"
|
||||
#include "MotorEncoder.h"
|
||||
#include "Car.h"
|
||||
|
||||
MotorEncoder m1(2, 8, 5, 4); //Initialise motor with pins
|
||||
MotorEncoder m2(3, 9, 11, 10); //Initialise motor with pins
|
||||
|
||||
Radio radio(120, 6, 7);
|
||||
MotorEncoder m1(2, A4, 5, 4, false, 120, 3.3); // Initialise motor with pins
|
||||
MotorEncoder m2(3, 9, 10, 8, true, 120, 3.3); // Initialise motor with pins
|
||||
|
||||
void encoder_handler_m1() {
|
||||
m1.wheelSpeed(); // Call wheelSpeed() of the m1 MotorEncoder instance
|
||||
m1.wheelSpeed(); // Call wheelSpeed() of the m1 MotorEncoder instance
|
||||
}
|
||||
|
||||
void encoder_handler_m2() {
|
||||
m2.wheelSpeed(); // Call wheelSpeed() of the m2 MotorEncoder instance
|
||||
m2.wheelSpeed(); // Call wheelSpeed() of the m2 MotorEncoder instance
|
||||
}
|
||||
|
||||
//Radio radio(120, 6, 7);
|
||||
|
||||
Car car(&m1, &m2, 110); // Initialise the car with motors and distance
|
||||
|
||||
void setup() {
|
||||
//Initialise motor with pins
|
||||
Serial.begin(9600);//Initialize the serial port
|
||||
|
||||
m1.EncoderHandlerInit(encoder_handler_m1);
|
||||
m1.setPoint = 80;
|
||||
m2.EncoderHandlerInit(encoder_handler_m2);
|
||||
m2.setPoint = 80;
|
||||
}
|
||||
Serial.println("Car started");
|
||||
car.turn(90, 20);
|
||||
//car.forward(100,50);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
m1.compute(1);//compute
|
||||
m1.advance();//Motor Forward
|
||||
m2.compute(1);//compute
|
||||
m2.advance();//Motor Forward
|
||||
}
|
Loading…
Reference in New Issue
Block a user