Admin: Created page with " #define pinLED 13 #define pinRX 0 #define pinTX 1 // MOTOR LEFT // power #define pinWheelL_PWMA 5 // dir #define pi..."

2018-05-20T21:52:28Z

Created page with " <syntaxhighlight lang=c style="border:3px dashed blue"> #define pinLED 13 #define pinRX 0 #define pinTX 1 // MOTOR LEFT // power #define pinWheelL_PWMA 5 // dir #define pi..."

New page

<syntaxhighlight lang=c style="border:3px dashed blue">

#define pinLED 13

#define pinRX 0
#define pinTX 1

// MOTOR LEFT
// power
#define pinWheelL_PWMA 5
// dir
#define pinWheelL_AIN1 8
#define pinWheelL_AIN2 9

// MOTOR RIGHT
// power
#define pinWheelR_PWMB 6
// dir
#define pinWheelR_BIN2 11
#define pinWheelR_BIN1 12

// ENCODER
#define pinWheelL_ENCD 2
#define pinWheelR_ENCD 3

// ULTRASOUND
#define pinUS_TRIG 10
#define pinUS_ECHO 4

#define pinWheelStandby 7

#define MAXPOWER 255
#define STEPS_L 5
#define STEPS_R 5
#define STEPS_B 10
#define STEPS_F 20

// A01,A02 // LEFT MOTOR
// B01,B02 // LEFT MOTOR

// Generally, you should use "unsigned long" for variables that hold time
// The value will quickly become too large for an int to store
const int Nt=2;
unsigned long t0_ms[Nt]; // will store last time LED was updated

// constants won't change :
long dt_ms[Nt]; // interval at which to do something

// ROBOT MODE

#define EXPLORER 0 // explorer
#define LFOLLOWER 1 // explorer

int robotMode=EXPLORER;

#define MAXSPEED 255
int speedR=MAXSPEED;
int speedL=MAXSPEED;

void setup() {
// put your setup code here, to run once:
Serial.begin(57600);

pinMode(LED_BUILTIN, OUTPUT);

pinMode(pinWheelL_AIN1, OUTPUT);
pinMode(pinWheelL_AIN2, OUTPUT);

pinMode(pinWheelR_BIN1, OUTPUT);
pinMode(pinWheelR_BIN2, OUTPUT);

// SONAR
pinMode(pinUS_TRIG, OUTPUT);
pinMode(pinUS_ECHO, INPUT);
//
pinMode(pinWheelStandby, OUTPUT);
digitalWrite(pinWheelStandby, HIGH);

dt_ms[0] = 10; // interval at which to avareage angular velocity
//Serial.println("> Ready!");
}

void loop() {

unsigned long t_ms = millis();
//
// check against collision system
if (t_ms - t0_ms[2] >= dt_ms[2]) {
// save the last time you did it
t0_ms[0] = t_ms;
int d=ultrasound();

setMotor(255,255);

if (d<10) {
Serial.println(d);

}
}
}

// Function to Spin Right
void setMotor(int speedL, int speedR) {
int speedLabs=abs(speedL);
int speedRabs=abs(speedR);

if (speedL>0) {
digitalWrite(pinWheelL_AIN1, LOW);
digitalWrite(pinWheelL_AIN2, HIGH);
}

if (speedL<0) {
digitalWrite(pinWheelL_AIN1, HIGH);
digitalWrite(pinWheelL_AIN2, LOW);
}

if (speedR>0) {
digitalWrite(pinWheelR_BIN1, LOW);
digitalWrite(pinWheelR_BIN2, HIGH);
}

if (speedR<0) {
digitalWrite(pinWheelR_BIN1, HIGH);
digitalWrite(pinWheelR_BIN2, LOW);
}

// set power
analogWrite(pinWheelL_PWMA, speedLabs);
analogWrite(pinWheelR_PWMB, speedRabs);
}

long ultrasound() {
digitalWrite(pinUS_TRIG, LOW); // Added this line
delayMicroseconds(2); // Added this line
digitalWrite(pinUS_TRIG, HIGH);
// delayMicroseconds(1000); - Removed this line
delayMicroseconds(10); // Added this line
digitalWrite(pinUS_TRIG, LOW);
long duration_us = pulseIn(pinUS_ECHO, HIGH);
long distance_cm = (duration_us/2) / 29.1;

if (distance_cm<200) {
Serial.println(distance_cm);
}
//delay(10);
return(distance_cm);
}

</syntaxhighlight>

RobotExplorerContubot1simple - Revision history

Admin: Created page with " #define pinLED 13 #define pinRX 0 #define pinTX 1 // MOTOR LEFT // power #define pinWheelL_PWMA 5 // dir #define pi..."