Main Page

  ____ ___  _   _ _____ _   _ ____   ___ _____     
 / ___/ _ \| \ | |_   _| | | | __ ) / _ \_   _|   
| |  | | | |  \| | | | | | | |  _ \| | | || |     
| |__| |_| | |\  | | | | |_| | |_) | |_| || |   
 \____\___/|_| \_| |_|  \___/|____/ \___/ |_|    
                                                                       
              CONTUbernio roBOTics

ROBOTS

Le 3 leggi della robotica

1. “Un robot non può recar danno a un essere umano né può permettere che a causa del proprio mancato intervento un essere umano riceva danno”.
2. “Un robot deve obbedire agli ordini impartiti dagli esseri umani purché tali ordini non contravvengano alla prima legge”.
3. “Un robot deve proteggere la propria esistenza purché questo non contrasti con la prima e la seconda legge”.

Isaac Asimov

THYMIO

Programmi preimpostati

	colore	programma
Intestazione riga 2	Riga 2, cella 2	Riga 2, cella 3
Intestazione riga 3	Riga 3, cella 2	Riga 3, cella 3

CONTUBOT1

Hardware

PIN ALIMENTAZIONE

CONTROLLER	PIN	DRIVER MOTORI
(+) PILA	VIN	VM
(-) PILA	GND	GND
	5V	VCC

PIN DIGITALI

CONTROLLER	PIN	DRIVER MOTORI
+ pila	VIN	X
- pila	GND	X
RX	D00	X
TX	D01	X
ENC1	D02	X
ENC2	D03	X
...	D04	-
...	D05	-
...	D06	-
...	D07	-
...	D08	-
...	D09	-
...	D10	-
...	D11	-
...	D12	-
LED	D13	-

Software

Esempio

#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);
}