Here is a short video we made that day:
However this also showed us nothing was perfect the first time, and still a lot of work had to be done. This optimization process for the engineers did mean that I couldn't test as much, and when we did we risked getting iron fillings in between the electronics... So I decided to let the boys do their work and started working on the master, the logic of the system.
Since a lot had changed over the course of the project my initial ideas didn't cut it anymore, so it was time for a new plan. I started by taking another look at the functions of the robot, and how they could be translated to the master and slaves. Figuring I could call an complex task with a simple command trough the use of a library of some sort, I came to the following schematic.
for this schematic, time progresses as you read down the page.
Master
send: start moving
ask: status
send: prepare
ask: status
send:slow down
ask: status
send: pick up bottle
ask:status
send: stand still
ask: status
send: start moving
ask: status
send: prepare with bottle
ask: status
send: slow down
ask: status
send: put down the bottle
ask: status
send: stand still
ask: status
send: start moving
ask: status
send: prep
ask: status
-end of track-
Slave Pick and Place
incoming:
-prepare
-pick up bottle
-prepare with bottle
-put down the bottle
outgoing:
-preparing
-picking up the bottle
-bottle acquired
-putting down the bottle
Slave Line tracking
incoming:
-start moving
-slow down
-stand still
outgoing:
-moving
-slowing down
-standing still
This chart gave me some more insight in that what had to be done in which order in time, but in order to let the master communicate this with the slaves I still needed an solution.
I got the idea one night just before i went to bed, bits! Not so much of an idea you might think, but for me it was what I needed. A simple bit can represent the whole of this information if both sides of the conversation have a library they can link it with.
This mindset quickly resulted in a new scheme.
S1(pick and place) status bits:
-scissor lift, move/stop (1/0)
-arm, left/right (1/0)
-hand, closed/open(1/0)
S2(line tracking) status bits:
-move, yes/no(1/0)
-slow down, yes/no(1/0)
stop, yes/no(1/0)
Putting my new ideas to the test trough an I2C bus showed me simply using the example code from arduino was not enough. So for this whole idea to work I had to write a new code that did work.
After some sniffing around on the internet i had it working:
Master code:
#include <Wire.h>
byte a[] = {0,0,0};
byte d[] = {0,0,0};
void setup()
{
Wire.begin();
Serial.begin(9600);
}
void loop()
{
Request();
delay(500);
Send();
delay(500);
}
void Send()
{
Wire.beginTransmission(2);
Wire.write(a, 3);
Wire.endTransmission();
Serial.print("a is ");
Serial.println(a[0]);
Serial.println(a[1]);
Serial.println(a[2]);
Serial.println(" ");
}
void Request()
{
Wire.requestFrom(2, 3);
byte index = 0;
while(Wire.available() > 0 && index < 3)
{
d[index] = Wire.read();
index++;
}
Serial.print("d is ");
Serial.println(d[0]);
Serial.println(d[1]);
Serial.println(d[2]);
Serial.println(" ");
}
The challenge in this code was the request function, the master can write a whole array in one line of code. But to read an array, a for loop must be written in order to assign the bits to the right memory.
For the slave the code looked like this:
#include <Wire.h>
byte a[] = {0,0,0};
byte d[] = {0,0,0};
void receiveEvent(int howMany);
void setup()
{
Wire.begin(2);
Wire.onReceive(receiveEvent);
Wire.onRequest(requestEvent);
Serial.begin(9600);
}
void loop()
{
delay(100);
}
void receiveEvent(int howMany)
{
byte index = 0;
while(Wire.available() > 0 && index < 3)
{
a[index] = Wire.read();
}
Serial.print("a is ");
Serial.println(a[0]);
Serial.println(a[1]);
Serial.println(a[2]);
Serial.println(" ");
}
void requestEvent()
{
Wire.write(d, 3);
Serial.print("d is ");
Serial.println(d[0]);
Serial.println(d[1]);
Serial.println(d[2]);
Serial.println(" ");
}
The interaction between these two is simply sending 3 zero's they receive from each other, although not significant a zero could be a one just as easy...
With the means of communication under control and working, I once more took a closer look on my bit library.
I needed a total of 4 arrays, 2 for each slave:
S1IN
S1OUT
S2IN
S2OUT
now on to how much information each array need to carry, in essence both the outgoing strings (from the master) needed to be only 2 bits large, since 2 bits means 4 options to store a command.
This led to the arrays being declared as:
S1OUT : 0[prepare/do not prepare], 0[engage with the bottle/do nothing]
S2OUT : 0[move/stop], 0[slow down, ignore bit]
The status bits could be simplified as well, and led to them being declared as:
S1IN : 0[ready/not ready], 0[IR signal/no signal], 0[bottle/no bottle]
S2IN : 0[moving/standing still], 0[slowing down/ignore bit]
Now I knew what I was working with, it was time to put it all together in a flowchart. My philosophy was that the processor would be running indefinitely fast, and the chart should at all times give the right task.
So after some puzzling I had this:
and for slave 1,
Now that i designed the master, it was time to write the final versions of my program.
//master test 2
#include <Wire.h>
#include <RunningAverage.h>
#define address 1
#define address2 2
int Usensor = A0;
int Usens;
unsigned long duration = 0;
int x;
int y;
RunningAverage myRA(10);
int samples = 0;
byte S1OUT[] = {0,0}; //slave 1 uitgaand, 0 is PREP, 1 is PAKFLES
byte S2OUT[] = {0,0}; //slave 2 uitgaand, 0 is RIJDEN, 1 is REMMEN
byte S1IN[] = {0,0,0,0}; //slave 1 inkomend, 0 is READY, 1 is IR, 2 is FLES, 3 FLESCOUNTER
byte S2IN[] = {0,0}; //slave 2 inkomend, 0 is BEWEGEN, 1 is KRUIS
byte index;
void Send1();
void Send2();
void Request1();
void Request2();
void setup()
{
Wire.begin(); //start I2C protocol
Serial.begin(9600);
pinMode(Usensor, INPUT);
}
void loop()
{
Request1(); //request status-update slave 1
Serial.print("request 1 ");
Serial.print("*** ");
Serial.print(S1IN[0]);
Serial.print(S1IN[1]);
Serial.print(S1IN[2]);
Serial.print(S1IN[3]);
Request2(); //request status-update slave 2
Serial.print("Request 2 ");
Serial.print(" -- ");
Serial.print(S2IN[0]);
Serial.print(S2IN[1]);
Usensing();
if(S2IN[0] == 1) //bewegen
{
while(S1IN[0] == 0) //niet ready
{
S1OUT[0]=1; //PREP
}
if(Usens ==1) //Ultrasoon sensor >45 cm?
{
S2OUT[1]=1; //remmen
if(S1IN[1] == 1) //IR signaal?
{
S2OUT[0]=0; //remmen =0
S2OUT[1]=0; //rijden =0
}
}
else if(Usens ==2) //Object > 45 cm
{
S2OUT[1]=1; //remmen =1
}
}
else //de robot staat stil
{
if(S2IN[1] == 1) //is er een kruis?
{
if(S1IN[0] == 1) //ready?
{
if(S1IN[2] ==1) //hebben we een fles?
{
if(S1IN[3] == 1) //zetten?
{
S1OUT[1] = 1; //PAKFLES
}
else
{
S2OUT[0] =1; //ga rijden
}
}
else
{
S1OUT[1] =1; //PAKFLES
}
}
else if (S1IN[2] ==1) //hebben we een fles?
{
S1OUT[0] =1; //PREP
S2OUT[0] =1; //Rijden
}
else
{
S1OUT[0] =1; //PREP
}
}
else
{
S2OUT[0]=1; //rijden
if(S1IN[0] ==0) //niet ready?
{
S1OUT[0] =1; //PREP
}
}
}
Send1(); //Stuur commando naar slave 1
Serial.print("Send ");
Serial.print(" -- ");
Serial.print(S1OUT[0]);
Serial.print(S1OUT[1]);
Send2(); //Stuur commando naar slave 2
Serial.print("Send 2 ");
Serial.print(" -- ");
Serial.print(S2OUT[0]);
Serial.println(S2OUT[1]);
delay(1000);
}
void Request1() //data request slave 1
{
Wire.requestFrom(address, 4);
byte index=0;
while(Wire.available() > 0 && index < 3) //loopt door alle bits en zet ze in een array
{
S1IN[index] = Wire.read();
index++;
}
}
void Request2() // data request slave 2
{
Wire.requestFrom(address2, 2);
byte index=0;
while(Wire.available() > 0 && index < 2) //loopt door alle bits en zet ze in een array
{
S2IN[index] = Wire.read();
index++;
}
}
int Usensing()
{
x= analogRead(A0); //lees het signaal van de Usensoren
duration = pulseIn(Usensor, LOW); //time de puls
myRA.addValue(duration);
samples++;
if (samples == 30) //reset het gemiddelde bij 30 samples
{
samples = 0;
myRA.clear();
}
if(x > 900) // Als de puls een constant signaal word
{
Usens = 1;
}
else if (myRA.getAverage() > 0.6) //als het gemiddelde van de pulsen boven de 0.6 licht
{
Usens = 2;
}
else
{
Usens = 0;
}
return(Usens);
}
void Send1() //stuur slave 1 aan
{
Wire.beginTransmission(1);
Wire.write(S1OUT, 1);
Wire.endTransmission();
}
void Send2() //stuur slave 2 aan
{
Wire.beginTransmission(2);
Wire.write(S2OUT, 2);
Wire.endTransmission();
}
This code involves the full flowchart as designed, the communication with the slaves, as well as the ultra sonic sensors in the front of the robot.
For pick and place I wrote this code, this also formed the template for line tracking which Mark was developing.
// Slave d1 versie 2
#include <Wire.h>
#include <Servo.h>
byte S1IN[] = {0, 0}; // Inkomend van master, 0 is PREP, 1 is PAKFLES
byte S1OUT[] = {0, 0, 0, 0}; //Uitgaand naar master, 0 is READY, 1 is IR, 2 is FLES, 3 is flescounter
Servo myservo; // declaraties arm
int Ventielopen = 6;
int Ventieldicht = 7;
int flescounter = 0;
int knopmax = 4; //declaraties schaarlift
int knopmin = 13;
int IRsensorpin = A2;
int range = analogRead(A2);
int MotorV = 11;
int MotorA = 3;
int Flexsensor = A0; //initiation hand
int Ventiel2open = 8;
int Ventiel2dicht = 9;
int flexfles = analogRead(A0);
int bladpos = 0; //counter die links en recht bijhoud
int ks;
int ks1;
int pos;
int mag = A1;
void setup()
{
Serial.begin(9600);
Wire.begin(1);
Wire.onReceive(receiveEvent);
Wire.onRequest(requestEvent);
myservo.attach(5); //setup arm
pinMode(Ventielopen, OUTPUT);
pinMode(Ventieldicht, OUTPUT);
pinMode(knopmax, INPUT); //setup schaarlift
pinMode(knopmin, INPUT);
pinMode(IRsensorpin, INPUT);
pinMode(MotorV, OUTPUT);
pinMode(MotorA, OUTPUT);
pinMode(Ventiel2open, OUTPUT); //setup hand
pinMode(Ventiel2dicht, OUTPUT);
pinMode(Flexsensor, INPUT);
pinMode(mag, OUTPUT);
}
void loop()
{
range = analogRead(A2);
if(range > 100)
{
S1OUT[1] = 1; //schrijf IR hoog
Serial.print("++ ");
Serial.print(range);
}
else
{
S1OUT[1] = 0; //schrijf IR laag
}
while(S1IN[0] == 1 || S1OUT[0] == 0)
{
PREP();
Serial.print("Start Prep ");
}
while(S1IN[1] == 1)
{
PAKFLES();
Serial.println("Start pakfles ");
}
}
void PREP()
{
while(Ventielopen != HIGH)
{
digitalWrite(Ventielopen, HIGH);
delay(50);
digitalWrite(Ventielopen, LOW):
}
do
{
ks = digitalRead(knopmax);
analogWrite(MotorV, 255);
analogWrite(MotorA, 0);
} while(ks != 1);
if(bladpos == 0)
{
digitalWrite(mag, HIGH);
for(pos = 0; pos < 180; pos += 1) // goes from 0 degrees to 180 degrees
{ // in steps of 1 degree
myservo.write(pos); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
delay(50);
S1OUT[0] = 1;
}
else
{
digitalWrite(mag, LOW);
for(pos = 180; pos>=1; pos-=1) // goes from 180 degrees to 0 degrees
{
myservo.write(pos); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
delay(50);
S1OUT[0] = 1;
}
}
int PAKFLES()
{
do
{
ks1 = digitalRead(knopmin);
range = analogRead(A2);
flexfles = analogRead(A0);
analogWrite(MotorA, 255);
analogWrite(MotorV, 0);
}while(range > 450 || ks1 != 0 || flexfles > 800);
if(S1OUT[2] == 0)
{
digitalWrite(Ventiel2open, HIGH);
delay(50);
digitalWrite(Ventiel2open, LOW);
S1OUT[2] = 1;
bladpos = 1;
S1OUT[3] = 1;
}
else
{
digitalWrite(Ventiel2dicht, HIGH);
delay(50);
digitalWrite(Ventiel2dicht, LOW);
S1OUT[2] = 0;
bladpos = 0;
S1OUT[3] = 0;
}
}
void requestEvent()
{
Wire.write(S1OUT, 4);
Serial.print(S1OUT[0]);
Serial.print(S1OUT[1]);
Serial.print(S1OUT[2]);
Serial.print(S1OUT[3]);
}
void receiveEvent(int howMany)
{
byte index = 0;
while(Wire.available()> 0 && index < 2)
{
S1IN[index] = Wire.read();
Serial.print("--");
Serial.print(S1IN[0]);
Serial.println(S1IN[1]);
}
}
With these codes written and confident they should work I went home for the weekend, Tommy was taken home to apply all wires, and with one week left it became a do or die mission.
Geen opmerkingen:
Een reactie posten