From c85b7dd1fd51fcda45f67822d7ff8085eb1f87a0 Mon Sep 17 00:00:00 2001 From: Gab_S Date: Wed, 15 Jan 2020 11:39:33 +0000 Subject: [PATCH] Upload files to 'Mapping' --- Mapping/Server | 106 +++++++++++++++++ Mapping/esp.ino | 208 ++++++++++++++++++++++++++++++++ Mapping/motor_arduino_garbage | 218 ++++++++++++++++++++++++++++++++++ Mapping/motortest_first.ino | 136 +++++++++++++++++++++ Mapping/tankdrive.h | 66 ++++++++++ 5 files changed, 734 insertions(+) create mode 100644 Mapping/Server create mode 100644 Mapping/esp.ino create mode 100644 Mapping/motor_arduino_garbage create mode 100644 Mapping/motortest_first.ino create mode 100644 Mapping/tankdrive.h diff --git a/Mapping/Server b/Mapping/Server new file mode 100644 index 0000000..3f3278b --- /dev/null +++ b/Mapping/Server @@ -0,0 +1,106 @@ +/** + * (./) udp.pde - how to use UDP library as unicast connection + * (cc) 2006, Cousot stephane for The Atelier Hypermedia + * (->) http://hypermedia.loeil.org/processing/ + * + * Create a communication between Processing<->Pure Data @ http://puredata.info/ + * This program also requires to run a small program on Pd to exchange data + * (hum!!! for a complete experimentation), you can find the related Pd patch + * at http://hypermedia.loeil.org/processing/udp.pd + * + * -- note that all Pd input/output messages are completed with the characters + * ";\n". Don't refer to this notation for a normal use. -- + */ + +// import UDP library +import hypermedia.net.*; +import java.util.*; +Deque d = new ArrayDeque(); + +String myString; +float x,y,angle; +float num; +float num2; + +UDP udp; // define the UDP object + +/** + * init + */ +void setup() { + size(820, 820); + noSmooth(); + background(0); + translate(410, 410); + stroke(255); + strokeWeight(3); + + // create a new datagram connection on port 6000 + // and wait for incomming message + udp = new UDP( this, 6000 ); + //udp.log( true ); // <-- printout the connection activity + udp.listen( true ); +} + +//process events +void draw() { + if (!d.isEmpty()) { + + String[] q = splitTokens(d.pop(), ","); + + + num=float(q[0]); // Converts and prints float + num2 = float(q[1]); // Converts and prints float + + //Pass from polars to cartesians adna dd 410 to be in the middle of the 820 by 820 screen. + angle = num * 0.0174533; + x = (sin(angle)*num2 + 410); + y = (cos(angle)*num2 + 410); + + } + if(num == 0 ) + { + background(0); + translate(410, 410); + } + point(x, y); + +} + +/** + * on key pressed event: + * send the current key value over the network + */ +void keyPressed() { + + String message = str( key ); // the message to send + String ip = "localhost"; // the remote IP address + int port = 6100; // the destination port + + // formats the message for Pd + message = message+";\n"; + // send the message + udp.send( message, ip, port ); + +} + +/** + * To perform any action on datagram reception, you need to implement this + * handler in your code. This method will be automatically called by the UDP + * object each time he receive a nonnull message. + * By default, this method have just one argument (the received message as + * byte[] array), but in addition, two arguments (representing in order the + * sender IP address and his port) can be set like below. + */ +// void receive( byte[] data ) { // <-- default handler +void receive( byte[] data, String ip, int port ) { // <-- extended handler + + + // get the "real" message = + // forget the ";\n" at the end <-- !!! only for a communication with Pd !!! + data = subset(data, 0, data.length); + String message = new String( data ); + d.add(message); + // print the result + +} diff --git a/Mapping/esp.ino b/Mapping/esp.ino new file mode 100644 index 0000000..b25b460 --- /dev/null +++ b/Mapping/esp.ino @@ -0,0 +1,208 @@ +#include +#include +#include +#include + +const char * networkName = "COSMOTE-424630"; +const char * networkPswd = "tQThvFVAxRJhsYKg"; +const char * udpAddress = "192.168.1.76"; +const int udpPort = 6000; +SFEVL53L1X distanceSensor; +WiFiUDP udp; +boolean connected = false; + +#define MOTOR_PHASE_A 26 +#define MOTOR_PHASE_B 25 +#define MOTOR_PHASE_C 33 +#define MOTOR_PHASE_D 32 +#define HOMING_PIN 35 +unsigned long starttime; +unsigned long endtime; +float rotationtime; +int count = 0; +int flag = 0; + +int circle = 2048 * 3.25; + +char *buffer1; +char buffer2[20]; +int distance; +int angle; + +int step(); + +void setup() { + Wire.begin(); + if (distanceSensor.begin() == 0) //Begin returns 0 on a good init + { + Serial.println("Sensor online!"); + } + + connectToWiFi(networkName, networkPswd); + Serial2.begin(9600); + Serial.begin(9600); + Serial.println("Setup"); + pinMode(MOTOR_PHASE_A, OUTPUT); + pinMode(MOTOR_PHASE_B, OUTPUT); + pinMode(MOTOR_PHASE_C, OUTPUT); + pinMode(MOTOR_PHASE_D, OUTPUT); + pinMode(HOMING_PIN, INPUT); + starttime = millis(); + buffer1 = (char*) malloc((sizeof(char) + sizeof(unsigned long)) * sizeof(char) + 1); + while (digitalRead(HOMING_PIN) == HIGH)step(); + while (digitalRead(HOMING_PIN) == LOW) { + step(); + } + delay(2000); +} + +void loop() { + //if(count%circle==0)delay(5000); + for(count=0;count +#include + +#define SONAR_PIN 10 +#define SERVO_PIN 9 +#define MAX_DISTANCE 200 //max sonar dinstance according to the type of sensor i use Hc-SR04 + +NewPing sonar(SONAR_PIN,SONAR_PIN, MAX_DISTANCE); +Servo myservo; + + + +int GetDistances(); + +int i; +int enr=5; // enable pin for motor r (must be pwm) +int in1=4; // pin for one of the two connections of the mottor +int in2=3;// other connection (these two are digital pins) +// depends on how you end up wiring and placing the motors it will spin a certain way +// and another way when you reverse the voltage , direct holds (0,1) depending on how you +// wired the motor to go "forwards" what ever that means for your project +int directr=0; +int enl=6; +int in3=7; +int in4=8; +int directl=0; + +char buffer1[(sizeof(char)+sizeof(unsigned long))*sizeof(char)+1]; + + char a; + unsigned long runforsmth; +Motorz motorR(enr,in1,in2,directr); +Motorz motorL(enl,in3,in4,directl); + +TankDrive mytank(&motorR,directr,&motorL,directl); + +void setup(){ + Serial.begin(9600); + motorR.SetSpeed(80); //im using diffrent motors (trying to simulate wheels so i figure things out ) + motorL.SetSpeed(80); // so that explains the diffrent speed in each motor + pinMode(LED_BUILTIN, OUTPUT); +} + + +void loop(){ + + digitalWrite(LED_BUILTIN,LOW); + + + + + + /* + * + * + * + * if(a=='1'){ + digitalWrite(LED_BUILTIN, HIGH); + // turn the LED on (HIGH is the voltage level) + } + else if(a=='0') + digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW + + * + * + * + * + * + * + * + * + * + mytank.TurnRight(); + delay(10000); + mytank.TurnLeft(); + delay(10000); + mytank.MoveForward(); + delay(10000); + + + + +motorR.Start(); +Serial.println(motorR.GetDirection()); +delay(1000); +motorR.ChangeDir(); +Serial.print("Changing Dir: "); +Serial.println(motorR.GetDirection()); + +Serial.print("F:"); +Serial.print(mytank.getFDirR()); +Serial.print(" B:"); +Serial.print(mytank.getBDirR()); +Serial.print(" "); +Serial.print("F:"); +Serial.print(mytank.getFDirL()); +Serial.print(" B:"); +Serial.println(mytank.getBDirL()); +mytank.MoveForward(); +delay(5000); +mytank.MoveBack(); +delay(5000); + + * delay(1000); +mytank.TurnLeft(); +delay(5000); +mytank.TurnRight(); +delay(5000); +mytank.MoveForward(); +delay(5000); + + +Serial.println("Getting distances..."); +i=GetDistances(); +Serial.println("Done"); + if(i==-1) + { + Serial.println("Turning Left..."); + run_for_millis(&mytank,'L',200); + } + else if(i==0) + { + Serial.println("Going Forward..."); + run_for_millis(&mytank,'M',200); + } + else{ + Serial.println("Turning Right..."); + run_for_millis(&mytank,'R',200); + } */ +} +void serialEvent() { + while (Serial.available()) { + Serial.readBytes(buffer1,sizeof(buffer1)); + sscanf( buffer1, "%c%lu",&a, &runforsmth); + run_for_millis(&mytank,a,runforsmth); + /*if(a=='L'){ + digitalWrite(LED_BUILTIN, HIGH); + delay(runforsmth); + }*/ + } +} +int GetDistances(){ + int pos; + int maxdist[2]; + int temp; + myservo.attach(SERVO_PIN); + maxdist[0]=-1; + + for (pos = 20; pos <= 160; pos += 1) { // goes from 0 degrees to 180 degrees + // in steps of 1 degree + myservo.write(pos); + temp=sonar.ping_cm(); + Serial.print("Distance:"); + Serial.print(temp); + Serial.print(" Angle:"); + Serial.println(pos); + + if(temp>maxdist[0]){ + maxdist[0]=temp; + maxdist[1]=pos; + } + delay(55); + } + for (pos = 160; pos >=20; pos -= 1) { // goes from 0 degrees to 180 degrees + myservo.write(pos); + delay(5); + } + myservo.detach(); + + if(maxdist[1]<70){ + return -1; + } + + else if(maxdist[1]>110){ + return 1; + } + + else return 0; + +} +void run_for_millis(TankDrive *tank,char ch,unsigned long timetorun){ + + unsigned long count; + switch(ch){ + case 'L': + count=millis(); + do{ + tank->TurnLeft();} + while(millis()-count<=timetorun); + tank->Stop(); + break; + + case 'F': + count=millis(); + do{ + tank->MoveForward();} + while(millis()-count<=timetorun); + tank->Stop(); + break; + case 'R': + count=millis(); + do{ + tank->TurnRight();} + while(millis()-count<=timetorun); + tank->Stop(); + break; + } + + + +} diff --git a/Mapping/motortest_first.ino b/Mapping/motortest_first.ino new file mode 100644 index 0000000..075cb02 --- /dev/null +++ b/Mapping/motortest_first.ino @@ -0,0 +1,136 @@ + +//very crude libraries with objects for motor and tandrive system (2 motors no steering) +//tandrive is an object that you attach the motor objects you make bellow made some functions +//in hope it will be more easier to read etc. + +#include "Motorz.h"; +#include "tankdrive.h"; +#include +#include + +#define SONAR_PIN 10 +#define SERVO_PIN 9 +#define MAX_DISTANCE 200 //max sonar dinstance according to the type of sensor i use Hc-SR04 + +NewPing sonar(SONAR_PIN,SONAR_PIN, MAX_DISTANCE); +Servo myservo; + + + +int GetDistances(); + +int i; +int enr=5; // enable pin for motor r (must be pwm) +int in1=4; // pin for one of the two connections of the mottor +int in2=3;// other connection (these two are digital pins) +// depends on how you end up wiring and placing the motors it will spin a certain way +// and another way when you reverse the voltage , direct holds (0,1) depending on how you +// wired the motor to go "forwards" what ever that means for your project +int directr=0; +int enl=6; +int in3=7; +int in4=8; +int directl=0; + +char buffer1[(sizeof(char)+sizeof(unsigned long))*sizeof(char)+1]; + + char a; + unsigned long runforsmth; +Motorz motorR(enr,in1,in2,directr); +Motorz motorL(enl,in3,in4,directl); + +TankDrive mytank(&motorR,directr,&motorL,directl); + +void setup(){ + Serial.begin(9600); + motorR.SetSpeed(80); //im using diffrent motors (trying to simulate wheels so i figure things out ) + motorL.SetSpeed(80); // so that explains the diffrent speed in each motor + pinMode(LED_BUILTIN, OUTPUT); +} + + +void loop(){ + + digitalWrite(LED_BUILTIN,LOW); +} +void serialEvent() { + while (Serial.available()) { + Serial.readBytes(buffer1,sizeof(buffer1)); + sscanf( buffer1, "%c%lu",&a, &runforsmth); + run_for_millis(&mytank,a,runforsmth); + /*if(a=='L'){ + digitalWrite(LED_BUILTIN, HIGH); + delay(runforsmth); + }*/ + } +} +int GetDistances(){ + int pos; + int maxdist[2]; + int temp; + myservo.attach(SERVO_PIN); + maxdist[0]=-1; + + for (pos = 20; pos <= 160; pos += 1) { // goes from 0 degrees to 180 degrees + // in steps of 1 degree + myservo.write(pos); + temp=sonar.ping_cm(); + Serial.print("Distance:"); + Serial.print(temp); + Serial.print(" Angle:"); + Serial.println(pos); + + if(temp>maxdist[0]){ + maxdist[0]=temp; + maxdist[1]=pos; + } + delay(55); + } + for (pos = 160; pos >=20; pos -= 1) { // goes from 0 degrees to 180 degrees + myservo.write(pos); + delay(5); + } + myservo.detach(); + + if(maxdist[1]<70){ + return -1; + } + + else if(maxdist[1]>110){ + return 1; + } + + else return 0; + +} +void run_for_millis(TankDrive *tank,char ch,unsigned long timetorun){ + + unsigned long count; + switch(ch){ + case 'L': + count=millis(); + do{ + tank->TurnLeft();} + while(millis()-count<=timetorun); + tank->Stop(); + break; + + case 'F': + count=millis(); + do{ + tank->MoveForward();} + while(millis()-count<=timetorun); + tank->Stop(); + break; + case 'R': + count=millis(); + do{ + tank->TurnRight();} + while(millis()-count<=timetorun); + tank->Stop(); + break; + } + + + +} diff --git a/Mapping/tankdrive.h b/Mapping/tankdrive.h new file mode 100644 index 0000000..45485b3 --- /dev/null +++ b/Mapping/tankdrive.h @@ -0,0 +1,66 @@ +#ifndef _TANKDRIVE_H // not #ifnotdef +#define _TANKDRIVE_H + +#include "Motorz.h" + +class TankDrive +{ + Motorz *motorL; + Motorz *motorR; + int forwardR,forwardL,backR,backL; + public: + TankDrive(Motorz *MR,int fr,Motorz *ML,int fl){ + motorL=ML; + motorR=MR; + forwardR=fr; + forwardL=fl; + if(fr==1)backR=0; + else backR=1; + if(fl==1)backL=0; + else backL=1; + } + void MoveForward(){ + motorR->SetDirection(forwardR); + motorL->SetDirection(forwardL); + motorR->Start(); + motorL->Start(); + } + void MoveBack(){ + motorR->SetDirection(backR); + motorL->SetDirection(backL); + motorR->Start(); + motorL->Start(); + } + void Stop(){ + + motorR->Stop(); + motorL->Stop(); + } + void TurnRight(){ + motorR->SetDirection(backR); + motorL->SetDirection(forwardL); + motorR->Start(); + motorL->Start(); + } + void TurnLeft(){ + motorR->SetDirection(forwardR); + motorL->SetDirection(backL); + motorL->Start(); + motorR->Start(); + } + int getFDirR(){ + return forwardR; + } + int getFDirL(){ + return forwardL; + } + int getBDirR(){ + return backR; + } + int getBDirL(){ + return backL; + } +}; + + +#endif _TANKDRIVE_H