Skip to content

Commit

Permalink
Merge to new version
Browse files Browse the repository at this point in the history
  • Loading branch information
@dedukun
dedukun committed Mar 20, 2019
1 parent 3b07c70 commit 1cc74ce
Show file tree
Hide file tree
Showing 27 changed files with 110 additions and 211 deletions.
5 changes: 5 additions & 0 deletions AccelStepper/AccelStepper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -645,3 +645,8 @@ void AccelStepper::stop()
move(-stepsToStop);
}
}

bool AccelStepper::isRunning()
{
return !(_speed == 0.0 || _targetPos == _currentPos);
}
6 changes: 4 additions & 2 deletions AccelStepper/AccelStepper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -456,8 +456,10 @@ class AccelStepper
/// move() or moveTo()
void computeNewSpeed();
void computeNewSpeed(int correction);



/// Checks to see if the motor is currently running to a target
/// \return true if the speed is not zero or not at the target position
bool isRunning();
protected:

/// \brief Direction indicator
Expand Down
37 changes: 28 additions & 9 deletions MMKIT/MMkit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,8 +40,8 @@ boolean Running=false;


int reading=0;
int _right_Correction=555;//-60
int _left_Correction=555; //90
int _right_Correction=550;//150
int _left_Correction=550; //150

float _leftSpeed = 474.0; //default _leftSpeed 500
float _rightSpeed = 474.0; //default _rightSpeed 500
Expand Down Expand Up @@ -89,7 +89,17 @@ void MMkit::readIRSensors() {
Serial.print(" = ");
Serial.println(IRsensorsValues[i]);
*/
}
}
current_cell.wall = B00000000; // clears cell values
if (isWallLeft() == 1) { // wall 0000 0 LRF checks if there is a left wall
current_cell.wall = current_cell.wall | B00000100;
}
if (isWallFront() == 1) { // wall 0000 0 LRF checks if there is a front wall
current_cell.wall = current_cell.wall | B00000001;
}
if (isWallRight() == 1) { // wall 0000 0 LRF checks if there is a right wall
current_cell.wall = current_cell.wall | B00000010;
}
}
void MMkit::testIRSensors() {
for (int i=0;i<4;i++){
Expand Down Expand Up @@ -183,8 +193,8 @@ void MMkit::rotate(float deg) {//positive rotates Right, negative rotates Left
_motorLeft.setCurrentPosition(0);
_motorRight.setCurrentPosition(0);
deg=deg/10;
_motorLeft.move(cmToSteps((deg/360.0)*3.14159265358979*(_WHEEL_SPACING+10)));
_motorRight.move(-cmToSteps((deg/360.0)*3.14159265358979*(_WHEEL_SPACING+10)));
_motorLeft.move(cmToSteps((deg/360.0)*3.14159265358979*_WHEEL_SPACING));
_motorRight.move(-cmToSteps((deg/360.0)*3.14159265358979*_WHEEL_SPACING));
Running=true;
}

Expand Down Expand Up @@ -230,7 +240,7 @@ void MMkit::waitForStart(){
readIRSensors();
delay(100);

while(IRsensorsValues[0]<450||(6000 / (IRsensorsValues[1]- 17) - 4 )<6) {
while(IRsensorsValues[0]<150||(6000 / (IRsensorsValues[1]- 17) - 2 )<10) {
readIRSensors();
digitalWrite(13,HIGH);
int Diference_Between_Right_Left=abs(IRsensorsValues[1]-IRsensorsValues[2]); // get the difference between right and left sensor
Expand Down Expand Up @@ -298,15 +308,15 @@ void MMkit::runSpeed() {
case B00000010: //Right wall
//Serial.println("Right Wall");
_motorRight.setSpeed(+(_rightSpeed-(_right_Correction-IRsensorsValues[1])));
_motorLeft.setSpeed(_leftSpeed+(_right_Correction-IRsensorsValues[1])/3);
_motorLeft.setSpeed(_leftSpeed+(_right_Correction-IRsensorsValues[1]));
break;
default:
_motorLeft.setSpeed(_leftSpeed);
_motorRight.setSpeed(+_rightSpeed);
break;
}
//Serial.println(IRsensorsValues[3]);
if(IRsensorsValues[0]>750&&IRsensorsValues[3]>750){
if(IRsensorsValues[0]>890&&IRsensorsValues[3]>890){
if(_motorRight.distanceToGo()>1000&&_motorLeft.distanceToGo()>1000){
if(current_cell.theta=0x02)current_cell.y++;
if(current_cell.theta=0x01)current_cell.x--;
Expand All @@ -317,7 +327,16 @@ void MMkit::runSpeed() {
_motorLeft.setCurrentPosition(0);
_motorRight.setCurrentPosition(0);
_motorLeft.setSpeed(+_leftSpeed);
_motorRight.setSpeed(_rightSpeed);
_motorRight.setSpeed(_rightSpeed);
// goForward(-1);
// while (_motorLeft.distanceToGo() < 0 || _motorRight.distanceToGo() < 0){
// //while (_motorLeft.isRunning() || _motorRight.isRunning()){
// _motorLeft.runSpeed();
// _motorRight.runSpeed();
// }
// readIRSensors();
// _motorLeft.setCurrentPosition(0);
// _motorRight.setCurrentPosition(0);

}
_motorLeft.runSpeed();
Expand Down
131 changes: 29 additions & 102 deletions MMKIT/MMkit.h
View file
Original file line number Diff line number Diff line change
@@ -1,15 +1,7 @@
// MMkit.h
//
/// \mainpage MMkit library for Arduino
///
/// This is the Arduino MMkit library.
/// It provides an object-oriented interface for Micromouse Kit Robot.
///
///
/// MMKIT version
/// in development
/// V1.0
///1 june 2016
/* MMKIT version to control motors
in development
V1.0
*/

#ifndef MMkit_h
#define MMkit_h
Expand All @@ -21,117 +13,56 @@ class MMkit
{
public:
// IR thresholds
#define IR_FRONT_L_THRESHOLD 320 //38 105
#define IR_FRONT_R_THRESHOLD 320 //38
#define IR_LEFT_THRESHOLD 390 //18
#define IR_RIGHT_THRESHOLD 390
#define IR_FRONT_L_THRESHOLD 600 //38 105
#define IR_FRONT_R_THRESHOLD 600 //38
#define IR_LEFT_THRESHOLD 350 //18
#define IR_RIGHT_THRESHOLD 350

#define _WHEEL_SPACING 81 //mm 81
#define _WHEEL_SPACING 81.0 //mm 81
#define _WHEEL_RADIUS 31.0 //mm 27.5

int IRsensorsValues[4];
boolean Correction_ON;
boolean Front_Detection_ON;
MMkit(AccelStepper leftMotor, AccelStepper rightMotor);

//!Description: The robot will try to move in its forward direction at the predefined

///forward motion speed. If a front wall is detected, the robot shall rotate right or left.
///Description: The robot will try to move in its forward direction at the predefined
///forward motion speed. If a front wall is detected, the robot shall rotate right or left.
///If walls are sensed on its left and right, it shall move parallel to them and
///in their middle. If only one wall is sensed, it shall move parallel to it.
///Input parameter: none
///Output parameter: none
void goForward(float cm);

//!Description: The robot will try to rotate over is axis positive right, negative values left

///Input parameter: desired angle in degrees
///Output parameter: none

void rotate(float deg);

///!Description: The robot shall rotate in it's axis the specified angle to the Right.

///Input parameter: desired angle in degrees
///Description: The robot shall rotate in it's axis the specified angle to the Right.
///Input parameter: desired angle in degrees
///Output parameter: none
void rotateRight (float deg);

//!Description: The robot shall rotate in it's axis the specified angle to the Right.

///Input parameter: desired angle in degrees
///Description: The robot shall rotate in it's axis the specified angle to the Right.
///Input parameter: desired angle in degrees
///Output parameter: none
void rotateLeft(float deg);


//!Description: Perform initial configuration

///Input parameter: none
///Description: Perform initial configuration
///Input parameter: none
///Output parameter: none
void setupMMkit();

//!Description: This function makes the motor do a step so must be call each time we want the motor to turn 1,8 degrees

///Input parameter: none
///Output parameter: none
void run();

//!Description: This function makes the motor to stop, break.

///Input parameter: none
///Output parameter: none
void stop();

//!Description: This function Set the value of the LEFT IR sensor to the wall(default is 555.

///Input parameter: Value of IR_LEFT to the wall
///Output parameter: none
void setIR_LEFT(float LeftCorrection);

//!Description: This function Set the value of the Right IR sensor to the wall(default is 555.

///Input parameter: Value of IR_RIGHT to the wall
///Output parameter: none
void setIR_RIGHT(float RightCorrection);

//!Description: Continuously checks for the start signal (hand movement close to the left forward distance sensor where a sequence of high, low and high distances are detected)

///Input parameter: none
///Description: Continuously checks for the start signal (hand movement close to the left forward distance sensor where a sequence of high, low and high distances are detected)
///Input parameter: none
///Output parameter: none
void waitForStart();

///!Description: Checks if the dstance has been reached, if so returns false else is still moving returns true

///Input parameter: none
///Output parameter: True -> moving False -> Stop
boolean running();

//!Description: Reads the values of the IRsensorsValues and stores the values in IRsensorsValues[0], IRsensorsValues[1], IRsensorsValues[2], IRsensorsValues[3]

///Input parameter: none
///Output parameter: none, values are stored at IRsensorsValues[],
void readIRSensors();

//!Description: Reads the values of the IRsensorsValues and stores the values in IRsensorsValues[0], IRsensorsValues[1], IRsensorsValues[2], IRsensorsValues[3]

///Input parameter: none
///Output parameter: Reads and Prints the values stored at IRsensorsValues[],
void testIRSensors();

//!Description: compares de treshold value to the readvalue and if this Value is Higher returns true

///Input parameter: none
///Output parameter: True if there is wall present
boolean isWallRight(void);

//!Description: compares de treshold value to the readvalue and if this Value is Higher returns true

///Input parameter: none
///Output parameter: True if there is wall present
boolean isWallLeft(void);

//!Description: compares de treshold value to the readvalue and if this Value is Higher returns true

///Input parameter: none
///Output parameter: True if there is wall present
boolean isWallFront(void);

/*********************/
Expand All @@ -143,7 +74,7 @@ struct cell
unsigned char y;
unsigned char theta;
unsigned char wall;
uint8_t weight;
unsigned char weight;
};
struct cell current_cell;
struct cell track[156];
Expand All @@ -159,28 +90,24 @@ struct cell
/// Output parameter: none
/// void setWHEEL_RADIUS(float WHEEL_RADIUS);

//! Description: Define the forward motion velocity of the robot

/// Input parameter: desired speed in cm/s
/// Description: Define the forward motion velocity of the robot
/// Input parameter: desired speed in cm/s
/// Output parameter: none
/// Used by:
void setForwardMotionSpeed (float speed);

//! Description: Get the configured forward motion velocity of the robot

/// Input parameter: none
/// Description: Get the configured forward motion velocity of the robot
/// Input parameter: none
/// Output parameter: speed in cm/s
float getForwardMotionSpeed(void);

//!Description: Define the forward motion acceleration of the robot.

///Input parameter: desired acceleration in cm/s
///Description: Define the forward motion acceleration of the robot.
///Input parameter: desired acceleration in cm/s
///Output parameter: none
void setForwardMotionAcceleration (float Acceleration);

//! Description: Get the configured forward motion acceleration of the robot.

/// Input parameter: none
/// Description: Get the configured forward motion acceleration of the robot.
/// Input parameter: none
/// Output parameter: acceleration in cm/s
float getForwardMotionAcceleration(void);

Expand Down
18 changes: 8 additions & 10 deletions MMKIT/examples/Advanced/FloodFillAlgorithm/Acceleration.ino
View file
Original file line number Diff line number Diff line change
@@ -1,12 +1,10 @@
/**@file acceleration.ino*/
void acceleration(void)
{
unsigned long currentMicros = micros();
if (currentMicros>= previousMicros + 900) {
previousMicros = currentMicros;
if (aceleration <= velocidade) {
Grigoras.setForwardMotionSpeed((int) aceleration);
aceleration = aceleration + aceleration / 100;
}
}
}
if (aceleration<=velocidade){
Grigoras.setForwardMotionSpeed((int) aceleration);
aceleration=aceleration+aceleration/100;
Serial.println((int) aceleration);
}else{
Serial.println((int) aceleration);
}
}
13 changes: 3 additions & 10 deletions MMKIT/examples/Advanced/FloodFillAlgorithm/FloodFillAlgorithm.ino
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@
/**@file*/
/*
This example was created by Sérgio Silva
on the 20th April 2015
Expand Down Expand Up @@ -31,7 +30,7 @@ enum statesRobotMovements {
STATE_MOV_STOP
};

uint8_t Map[16][16] ={
int Map[16][16] ={
{ 16, 15, 14, 13, 12, 11, 10, 9, 9, 10, 11, 12, 13, 14, 15, 16 },
{ 15, 14, 13, 12, 11, 10, 9, 8, 8, 9, 10, 11, 12, 13, 14, 15 },
{ 14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14 },
Expand All @@ -57,7 +56,6 @@ boolean turn=false;
unsigned int Step=0;
unsigned int velocidade=10;
double aceleration=1; //usada para chegar a velocidade
unsigned long previousMicros=0;

void setup(){
Grigoras.current_cell.x = 5; // Start x position
Expand Down Expand Up @@ -85,7 +83,6 @@ void loop(){
else{
digitalWrite(13,LOW);
turn=false;
Grigoras.readIRSensors();
toMove = nextMove();
robotMove();

Expand Down Expand Up @@ -134,14 +131,10 @@ void checkTrack(){

void reMap() {
for (int i = Step; i >= 0; i--) {
if((Grigoras.track[i].wall==B00000101)||(Grigoras.track[i].wall==B00000110)||(Grigoras.track[i].wall==B00000011)){
Grigoras.track[i].weight++;
Map[Grigoras.track[i].y][Grigoras.track[i].x] = Grigoras.track[i].weight;
}else{
Grigoras.current_cell.weight++;
break;
}
}
Grigoras.current_cell.weight++;
}
}


Loading

0 comments on commit 1cc74ce

Please sign in to comment.