1 changed files with 259 additions and 0 deletions
@ -0,0 +1,259 @@ |
|||
#include <Wire.h> // Used to speak with the VL53L1X through I2C |
|||
#include <WiFi.h> |
|||
#include <WiFiUdp.h> |
|||
#include <SparkFun_VL53L1X.h> // Measuring senssor's lib |
|||
|
|||
const char * networkName = "YOUR_WIFI_SSID"; |
|||
const char * networkPswd = "WIFI_PASS"; |
|||
const char * udpAddress = "SERVER_IP"; |
|||
const int udpPort = 6000; //SERVER LISTENING PORT
|
|||
|
|||
SFEVL53L1X distanceSensor; // creating object of sensor
|
|||
WiFiUDP udp; |
|||
boolean connected = false; |
|||
|
|||
#define MOTOR_PHASE_A 26 //SERVO MOTOR PIN1
|
|||
#define MOTOR_PHASE_B 25 //SERVO MOTOR PIN2
|
|||
#define MOTOR_PHASE_C 33 //SERVO MOTOR PIN3
|
|||
#define MOTOR_PHASE_D 32 //SERVO MOTOR PIN4
|
|||
#define HOMING_PIN 35 // HALL SENSOR INPUT PIN
|
|||
|
|||
//counting how many steps the servo has made zeros after full 360 measuring
|
|||
//circle
|
|||
int count = 0; |
|||
|
|||
//it takes 2048 for the stepper motor to rotate once and 3.25 turns of
|
|||
//the stepper motor to rotate the top platform once,thus taking the 360 measurments
|
|||
int circle = 2048 * 3.25; |
|||
|
|||
char *buffer1;//Buffer used to talk with Arduino
|
|||
char *buffer2;//Buffer used to send data to udp
|
|||
float distance;// distance measured
|
|||
int angle;// angle of measured distance
|
|||
float x,y; // the coordinates of the robot
|
|||
// a function created to implement FULL STEPPING in a stepper motor
|
|||
// you can implement your own version based on the stepper you have
|
|||
// maybe even try HALF STEPPING or get a ready made library
|
|||
int step(); |
|||
|
|||
|
|||
//BootStraping the system
|
|||
void setup() { |
|||
Wire.begin();//Begin I2C so we can talk with the measuring sensor
|
|||
|
|||
//Bringing the sensor online
|
|||
if (distanceSensor.begin() == 0) //Begin returns 0 on a good init
|
|||
{ |
|||
Serial.println("Sensor online!"); |
|||
} |
|||
//Sets it to max 1.3m works better with diffrent ambient lights
|
|||
//if you comment this out the sensor will work on 4m but won't be as accurate
|
|||
distanceSensor.setDistanceModeShort(); |
|||
//Default is 100 ,the bigger the number you set in the budget the more accurate
|
|||
//readings you'll get at the cost of time per measurment IntermeasurementPeriod
|
|||
//must always be greater or equal than budget,accepted budget values are:
|
|||
/*
|
|||
15 |
|||
20 |
|||
33 |
|||
50 |
|||
100 (default) |
|||
200 |
|||
500 |
|||
15 ms only works with Short distance mode. 100 ms is the default value |
|||
Usefull websites: |
|||
https://www.st.com/content/ccc/resource/technical/document/user_manual/group1/63/e0/7d/f1/0e/52/4d/cd/DM00562924/files/DM00562924.pdf/jcr:content/translations/en.DM00562924.pdf
|
|||
https://learn.sparkfun.com/tutorials/qwiic-distance-sensor-vl53l1x-hookup-guide/all
|
|||
*/ |
|||
distanceSensor.setTimingBudgetInMs(100); |
|||
distanceSensor.setIntermeasurementPeriod(100); |
|||
//connecting to wifi
|
|||
connectToWiFi(networkName, networkPswd); |
|||
//Serial2 is used for talkign with Arduino make sure to set the baud rate
|
|||
//(9600) the same on both devices.
|
|||
Serial2.begin(9600); |
|||
//Serial to print through USB for debugging purposes while you develop
|
|||
Serial.begin(9600); |
|||
Serial.println("Setup"); |
|||
//Setting output pins for stepper motor
|
|||
pinMode(MOTOR_PHASE_A, OUTPUT); |
|||
pinMode(MOTOR_PHASE_B, OUTPUT); |
|||
pinMode(MOTOR_PHASE_C, OUTPUT); |
|||
pinMode(MOTOR_PHASE_D, OUTPUT); |
|||
//Setting input pin for HALL SENSOR
|
|||
pinMode(HOMING_PIN, INPUT); |
|||
//Setting the size of buffer that will talk with the arduino
|
|||
//Since the messages Sent to arduino are of the type :
|
|||
//"L,100" - Rotates left for 100 ms
|
|||
//"F,1000" - Moves Forward for 1 sec
|
|||
// "R,500" -rotates right for 500 ms
|
|||
//We need the size of the character L,R,F + size of variable for time
|
|||
//multiplying by char (not neccesery) + 1 for terminating character
|
|||
buffer1 = (char*) malloc((sizeof(char) + sizeof(unsigned long)) * sizeof(char) + 1); |
|||
//Same here for talking with the udp server but this time the messages are
|
|||
//of the type:
|
|||
//"1,14.5,0,5" meaning :
|
|||
//Angle=1 degree
|
|||
//distance measured=14.5 cm
|
|||
//x coordinate=0; in cm
|
|||
//y coordinate=5 in cm
|
|||
buffer2 = (char*) malloc((sizeof(int)+(3*sizeof(float)) ) * sizeof(char) + 1); |
|||
//For the particular Hall Sensor and magnet setup this two while loops are
|
|||
//used in order to bring the top platform in the 0 degrees position (homing
|
|||
//procedure) from any position it is. You might find with your Hall senosor
|
|||
// and magnet setup that you need to change from HIGH to LOW etc. it is
|
|||
//suggested that you experiment and use printfs to see how your sensor and
|
|||
//magnet combo behaves so you can create your homing procedure
|
|||
//the Sensor outputs HIGH when it meets the magnet at 0 degrees but
|
|||
//i noticed that sometimes when the system truns on the sensor would be
|
|||
//reading HIGH meaning it was latched on high so the first loop turns until
|
|||
//the sensor unlatches and then till it latches again at 0 position this time
|
|||
//meaing at worst case might take 2 turns for the homing procedure to end.
|
|||
//The hall sensor used is : US1881
|
|||
while (digitalRead(HOMING_PIN) == HIGH)step(); |
|||
while (digitalRead(HOMING_PIN) == LOW) { |
|||
step(); |
|||
} |
|||
//Since it's a Dead reckoning system we start at 0,0
|
|||
x=0; |
|||
y=0; |
|||
} |
|||
|
|||
void loop() { |
|||
//Do one full circle of the top platform taking measurments.
|
|||
for(count=0;count<circle-1;){ |
|||
|
|||
// every 18 steps is almost a degree (360/2048 * 3.25)*18=0.973557692 degrees
|
|||
if (count % 18 == 0) { |
|||
delay(50); //precautionary delay so that IntermeasurementPeriod is met
|
|||
distanceSensor.startRanging(); |
|||
distanceSensor.stopRanging(); |
|||
//while loop waiting until ranging data is ready
|
|||
while(!distanceSensor.checkForDataReady()){}; |
|||
//Get the result of the measurement from the sensor
|
|||
distance = distanceSensor.getDistance(); |
|||
//if the ranging result is good Status is 0 if it's out of range
|
|||
//(above 1.3m for short or 4m for long ) it returns 4 check links above
|
|||
//for more info or look for the st's manual guide to VL53L1X
|
|||
if(distanceSensor.getRangeStatus()==0){ |
|||
//check connection to wifi
|
|||
if (connected) { |
|||
//Send a packet
|
|||
udp.beginPacket(udpAddress, udpPort); |
|||
// every steps is (360/2048 * 3.25)=0.054086538 degrees
|
|||
angle = ceil(count * 0.054086538); |
|||
//creating the message to send to server distance/10
|
|||
//to go from millis to cm
|
|||
sprintf(buffer2, "%d,%f,%f,%f",angle,distance/10,x,y); |
|||
udp.printf("%s",buffer2); |
|||
udp.endPacket(); |
|||
} |
|||
} |
|||
// Serial.print("ANGLE:");
|
|||
// Serial.print(angle);
|
|||
// Serial.print(" DISTANCE:");
|
|||
// Serial.println(distance/10);
|
|||
// Serial.println(buffer2);
|
|||
} |
|||
//the stepping function returns 1 so the counter is increased after stepping
|
|||
count += step(); |
|||
} |
|||
//Zero count since a full circle has been made
|
|||
count = count % circle-1; |
|||
|
|||
/* implement your Algorithm for mapping here
|
|||
In our case moving the motors forward for 1 sec tended to move the |
|||
robot for a distance of 12 cm and for a 90 degree rotation it took |
|||
850 milliseconds.So for example sending these two messages at arduino |
|||
"L,850","F,1000" will move the robot left for 12 cm. so given the |
|||
commands above on how to move the arduino and a way to get distances of |
|||
each angle you can make an algorithm that makes the robot move in a way |
|||
you want base on furthest distance or what ever you see fit. |
|||
|
|||
|
|||
*/ |
|||
|
|||
} |
|||
|
|||
|
|||
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; |
|||
} |
|||
} |
|||
Loading…
Reference in new issue