Gab_S
/
Slam_cars_stuff_cs161083_cs141296_cs131083
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Browse Source

Upload files to 'Mapping'

master
Gab_S 5 years ago
parent
08e718daba
commit
c85b7dd1fd
5 changed files with 734 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 106
      Mapping/Server
  2. 208
      Mapping/esp.ino
  3. 218
      Mapping/motor_arduino_garbage
  4. 136
      Mapping/motortest_first.ino
  5. 66
      Mapping/tankdrive.h

106
Mapping/Server
View File

@ -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<String> d = new ArrayDeque<String>();
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
}

208
Mapping/esp.ino
View File

@ -0,0 +1,208 @@
#include <Wire.h>
#include <WiFi.h>
#include <WiFiUdp.h>
#include <SparkFun_VL53L1X.h>
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<circle-1;){
count += step();
if (count % 18 == 0) {
distanceSensor.startRanging();
distance = distanceSensor.getDistance(); //Get the result of the measurement from the sensor
distanceSensor.stopRanging();
if (connected) {
//Send a packet
udp.beginPacket(udpAddress, udpPort);
angle = ceil(count * 0.054086538);
sprintf(buffer2, "%d,%f",angle,(float)distance);
udp.printf("%s",buffer2);
udp.endPacket();
}
Serial.print("ANGLE:");
Serial.print(angle);
Serial.print(" DISTANCE:");
Serial.println(distance/10);
Serial.println(buffer2);
}
}
// if(digitalRead(HOMING_PIN)==LOW)
// Serial.println("LOW");
// else
// Serial.println("HIGH");
count = count % circle-1;
sprintf (buffer1, "%c%lu",'F',2500);
Serial2.print(buffer1);
delay(5000);
/*
sprintf (buffer1, "%c%lu",'L',200);
Serial2.print(buffer1);
delay(2000);
sprintf (buffer1, "%c%lu",'F',200);
Serial2.print(buffer1);
delay(2000);
sprintf (buffer1, "%c%lu",'R',200);
Serial2.print(buffer1);
delay(2000);*/
/*
if(count==2048*3.25){
endtime=millis();
rotationtime=(endtime-starttime)/1000;
if(flag==0){
Serial.print("It took:");
Serial.print(rotationtime);
Serial.println("seconds");
flag++;
}
}
count=count%2048*3.25; */
}
int step() {
static int count;
switch (count) {
case 0:
{
digitalWrite(MOTOR_PHASE_D, HIGH);
digitalWrite(MOTOR_PHASE_C, LOW);
digitalWrite(MOTOR_PHASE_B, LOW);
digitalWrite(MOTOR_PHASE_A, LOW);
}
break;
case 1:
{
digitalWrite(MOTOR_PHASE_D, LOW);
digitalWrite(MOTOR_PHASE_C, HIGH);
digitalWrite(MOTOR_PHASE_B, LOW);
digitalWrite(MOTOR_PHASE_A, LOW);
}
break;
case 2:
{
digitalWrite(MOTOR_PHASE_D, LOW);
digitalWrite(MOTOR_PHASE_C, LOW);
digitalWrite(MOTOR_PHASE_B, HIGH);
digitalWrite(MOTOR_PHASE_A, LOW);
}
break;
case 3:
{
digitalWrite(MOTOR_PHASE_D, LOW);
digitalWrite(MOTOR_PHASE_C, LOW);
digitalWrite(MOTOR_PHASE_B, LOW);
digitalWrite(MOTOR_PHASE_A, HIGH);
}
break;
}
delay(8);
count++;
count = count % 4;
return 1;
}
void connectToWiFi(const char * ssid, const char * pwd) {
Serial.println("Connecting to WiFi network: " + String(ssid));
// delete old config
WiFi.disconnect(true);
//register event handler
WiFi.onEvent(WiFiEvent);
//Initiate connection
WiFi.begin(ssid, pwd);
Serial.println("Waiting for WIFI connection...");
}
//wifi event handler
void WiFiEvent(WiFiEvent_t event) {
switch (event) {
case SYSTEM_EVENT_STA_GOT_IP:
//When connected set
Serial.print("WiFi connected! IP address: ");
Serial.println(WiFi.localIP());
//initializes the UDP state
//This initializes the transfer buffer
udp.begin(WiFi.localIP(), udpPort);
connected = true;
break;
case SYSTEM_EVENT_STA_DISCONNECTED:
Serial.println("WiFi lost connection");
connected = false;
break;
default: break;
}
}

218
Mapping/motor_arduino_garbage
View File

@ -0,0 +1,218 @@
//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 <NewPing.h>
#include <Servo.h>
#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;
}
}

136
Mapping/motortest_first.ino
View File

@ -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 <NewPing.h>
#include <Servo.h>
#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;
}
}

66
Mapping/tankdrive.h
View File

@ -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
Write Preview
Loading…
Cancel
Save