The CCM trophy challenge, time to make it worth.

4:00, Yerseke.
Barely awake the whole team gathered in the town of Yerseke, to continue working on Tommy the final hours before the race. Continuing to test line tracking, and tweaking some processes took most of the morning, at around 6:00 I could finally do my first full test. To no surprise really it did not go as planned, in the beginning sensors where reading data from objects that resulted in wrong commando's being send to the slaves. After some small changes not much improvement, so the other systems got some more attentions while I took another look at the code.
By this time it was already time to get going however, so we started parking and prepared to move out to the town of Eindhoven.
So around 13:00 or so we had all our material set up and as good as ready at the marked, where the race was organized to start at 18:15, still some time.

Here is a photo album from the CCM trophy challenge.

During the trip I had taken some time to look once more to the code, so when we where set up I could resume testing again. this time I could pinpoint errors better, and I started fixing them quickly. It was however the third error I found that blew me away, the scissor lift refused to stop, or even read the button. While the stand alone program had no problem at at, we used it to lower it back down, the implemented version came with an error I couldn't understand, and with me my teachers. Trying to desperately get it working, it would not matter, so I was forced to take a break by my team, and let Mark work on line tracking.

This is the code as it was updated by testing:

// Slave d1 versie 3

#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 = 13;    //declaraties schaarlift
int knopmin = 12;
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()
{
  int ks = 0;    // knopschakelaar laag schrijven
  
  
  while(S1IN[0] == 1 && S1OUT[0] == 0)
  {
    Serial.print("Start Prep  ");
     PREP();
     
  }
  while(S1IN[1] == 1)
  {
    Serial.println("Start pakfles  ");
      PAKFLES();
      
    }
}

void PREP()
{
    range = analogRead(A2);
  if(range > 100)
  {
    S1OUT[1] = 1;    //schrijf IR hoog
    Serial.print("++ ");
    Serial.print(range);
  }
  else
  {
    S1OUT[1] = 0;    //schrijf IR laag
  }
  
  
    Serial.print(" Ventiel open ");
    digitalWrite(Ventielopen, HIGH);
    delay(50);
    digitalWrite(Ventielopen, LOW);
  
  do
  {
    Serial.print("Start schaarlift ");
    analogWrite(MotorV, 255);
    analogWrite(MotorA, 0);
    ks = digitalRead(knopmax);
  } 
  while(ks != 1);                                                           //This is where my code crashed.
  
  analogWrite(MotorV, 0);
  analogWrite(MotorA, 0);
  delay(10);
  
  if(bladpos == 0)
  {
    Serial.print(" Servo positie ");
    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 < 1)
  {
    S1IN[index] = Wire.read();
    Serial.print("--");
    Serial.print(S1IN[0]);
    Serial.println(S1IN[1]);
  }
}
  
After regaining some of my focus, taking a look around and talking with some people I started to think about some workarounds. Yet with barely any sleep last night it was willpower and not sanity that kept me going, and I lost most of the oversight in the system, working from error to error.

Then when the races started we had a half working robot. We where not the only one however, a lot of robots where experiencing problems, so it was decited by CCM that the race would take 3 minutes, and when no winner presented itself the robot that did best won.

Because of this we decided to enter the group stage with only line tracking, if it could move we had a chance.

Sadly we lost 3 of the 4 matches, with the biggest problem the carpet on the floor of the race area, our robot had to move at a certain speed if it wanted to move, but at this speed it could not accurately correct his movement...

After the second race I had written some new codes in order to hopefully fix the error.

// 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 IRwaarde;
int IRwaarde1;
int IRwaarde2;
int IRgem;

int knopmax = 13;                      //declaraties schaarlift
int knopmin = 12;
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 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()
{
  int ks = 0;    // knopschakelaar laag schrijven
  
  
  while(S1IN[0] == 1 && S1OUT[0] == 0)
  {
    Serial.print("Start Prep  ");
     PREP();
     
  }
  while(S1IN[1] == 1)
  {
    Serial.println("Start pakfles  ");
      PAKFLES();
      
    }
}

void PREP()
{
    range = analogRead(A2);
  if(range > 100)
  {
    S1OUT[1] = 1;    //schrijf IR hoog
    Serial.print("++ ");
    Serial.print(range);
  }
  else
  {
    S1OUT[1] = 0;    //schrijf IR laag
  }
  
  
    Serial.print(" Ventiel open ");
    digitalWrite(Ventielopen, HIGH);
    delay(50);
    digitalWrite(Ventielopen, LOW);
    int schaarhoog;
  do
  {
  analogWrite(MotorV, 255);
  analogWrite(MotorA, 0);
  range = analogRead(A2);
  }
  while(IRgem != range);
  analogWrite(MotorA, 0);
  analogWrite(MotorV, 0);
  delay(100);
  
  if(bladpos == 0)
  {
    Serial.print(" Servo positie ");
    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(25);                       // 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()
{
  IRwaarde = analogRead(A2);
  delay(100);
  IRwaarde1 = analogRead(A2);
  delay(100);
  IRwaarde2 = analogRead(A2);
  IRgem = ((IRwaarde + IRwaarde2 + IRwaarde1)/3);
  int schaarlaag;
  do
  {
    schaarlaag = digitalRead(knopmin);
    range = analogRead(A2);
    flexfles = analogRead(A0);
    analogWrite(MotorA, 255);
    analogWrite(MotorV, 0);
  }while(range > 450 || ks1 != 0 || flexfles > 800);
  analogWrite(MotorA, 0);
  analogWrite(MotorV, 0);
  
  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 < 1)
  {
    S1IN[index] = Wire.read();
    Serial.print("--");
    Serial.print(S1IN[0]);
    Serial.println(S1IN[1]);
  }
}

Using local declarations in stead of global, and changing the flow of the program a bit, it did not matter. We even came up with the idea of using the IR sensor to measure the height when it started descending, to return to the same height after picking up the bottle.
All to no avail... and i suspect it was because I couldn't think straight no more.

In the end, for the last race in the group stage I got the assignment from my team to write one last code, only for pick and place, no master. If needed they would put the robot in the right position, as long as it had a change to go for the bottle.
So I scavenged all my previous codes and put something together...

//Pick and place 05
int schaarliftA = 3;
int schaarliftV = 11;

int knopmin = 12;
int knopmax = 13;
int x = 1;
int m;
int n;

int pin1 = 6;
int pin2 = 7;
int pin3 = 8;
int pin4 = 9;

int Ventielopen = 6;
int Ventieldicht = 7;
int Ventiel2open = 8;
int Ventiel2dicht = 9;

#include <Servo.h> 

Servo myservo;  // create servo object to control a servo 
                // a maximum of eight servo objects can be created 

int pos = 0;    // variable to store the servo position 

void setup()
{
  Serial.begin(9600);
  pinMode(schaarliftV, OUTPUT);
  pinMode(schaarliftA, OUTPUT);
  pinMode(knopmin, INPUT);
  pinMode(knopmax, INPUT);
  myservo.attach(5);  
  pinMode(pin1, OUTPUT);
  pinMode(pin2, OUTPUT);
  pinMode(pin3, OUTPUT);
  pinMode(pin4, OUTPUT);
}

void loop()
{
  do
  {
    analogWrite(schaarliftV, 255);
    analogWrite(schaarliftA, 0);
    n = digitalRead(knopmax);
    Serial.print("knopmax is ");
    Serial.print(n);
    Serial.println(" ");
  }
  while(n !=1);
  analogWrite(schaarliftV, 0);
  analogWrite(schaarliftA, 0);
  delay(2000);
  
  digitalWrite(A1, 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(20);                       // waits 15ms for the servo to reach the position 
  } 
  digitalWrite(Ventielopen, HIGH);
  delay(50);
  digitalWrite(Ventielopen, LOW);
  int range;
  range= analogRead(A2);
  int flexfles;
  do
  {
    range=analogRead(A2);
    Serial.print(range);
  }
  while(range > 100);
  if(range > 100)
  {
    do
  {
    analogWrite(schaarliftA, 255);
    analogWrite(schaarliftV, 0);
    m = digitalRead(knopmin);
    Serial.print("knopmin is ");
    Serial.print(m);
    Serial.println(" ");
  }
  while(m != 0 || range > 450);
  analogWrite(schaarliftV, 0);
  analogWrite(schaarliftA, 0);
  
  flexfles = analogRead(A0);
  if(flexfles > 800)
  {
    digitalWrite(Ventiel2open, HIGH);
    delay(50);
    digitalWrite(Ventiel2open, LOW);
    delay(1000);
  }
  do
  {
    analogWrite(schaarliftV, 255);
    analogWrite(schaarliftA, 0);
    n = digitalRead(knopmax);
    Serial.print("knopmax is ");
    Serial.print(n);
    Serial.println(" ");
  }
  while(n !=1);
  analogWrite(schaarliftV, 0);
  analogWrite(schaarliftA, 0);
  digitalWrite(A1, LOW);
  
  do
  {
  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(20);                       // waits 15ms for the servo to reach the position 
  } 
  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(20);                       // waits 15ms for the servo to reach the position 
  }
  } 
  while(x=1);
  }
}
  
With the idea to make it a linear piece of software to simply go trough all the steps. And this is also why it didn't work, the loop was to small so it kept running like a madman, resulting in the arm swaying from left to right and nothing else happening.
After losing the final race that was it for us. Tommy would not continue in the competition. Even tough it was a bummer we gave everything we had, we all made mistakes and that's life...

With all this however, I still learned an enormous amount. Half a year ago I was still trying to gasp the idea of a robot and how it would all work in code. Now I wrote one of the things all by my own design. What also was an enormous help was the team I worked with. Never ever has something bad been said about another team member, we all had our responsibility and everyone took his. There was a team spirit like no other, and in my opinion it is this that has made this project the best in the past 2 years for me.

After a good weekend of sleep it is also time to look back and see what went wrong where, and I think the keyword is underestimation. If we had pushed some milestones earlier in time I think we would have gotten it working. It was the idea of  "one more day and we got this", if we could have started testing programs sooner, the result would have been different.

I will leave you with some pictures of Tommy, all done and ready for racing!

    
    
  

We are getting close! System test.

With the mechanics finally working in order, me and Mark claimed the robot and locked ourselves in the basement. With everything installed and wired we had to do a full test again, does the signal get where it should be? Since it had been a while, and of course a lot had changed again, we found more and more errors, slowly working our way trough them. For me this mend writing short programs that could read the signals, or send them. Tweaking a lot of the code in the process quickly resulted in updated versions of pick and place, the scissor lift, valve control, and the servo control.

//d2 input controll
int sR = A1;
int sL = A0;
int sM = A2;

int bL = 13;
int bR = 2;
int led = 8;

int pin = 11; //S links
int pin2 = 12;  // S rechts

int kL;
int kR;

int r;
int l;

int R;
int L;
int M;

void setup()
{
  Serial.begin(9600);
  pinMode(sR, INPUT);
  pinMode(sL, INPUT);
  pinMode(sM, INPUT);
  pinMode(bR, INPUT);
  pinMode(bL, INPUT);
  pinMode(pin, INPUT);
  pinMode(pin2, INPUT);
  pinMode(led, OUTPUT);
  digitalWrite(led, HIGH);
}

void loop()
{
  digitalWrite(led, HIGH);
  R = analogRead(sR);
  L = analogRead(sL);
  M = analogRead(sM);
  
  r = digitalRead(bR);
  l = digitalRead(bL);
  
  kL = digitalRead(pin2);
  kR = digitalRead(pin);
  
  Serial.print(R);
  Serial.print(" ");
  Serial.print(L);
  Serial.print(" ");
  Serial.print(M);
  Serial.print("  ");
  Serial.print(r);
  Serial.print(" ");
  Serial.print(l);
  Serial.print(" ");
  Serial.print(kR);
  Serial.print(" ");
  Serial.println(kL);
  delay(100);
}

A quick code to read the signal from line tracking, these are 3 LDR sensors and 2 buttons. 

// schaarlift test 2.0 GESLAAGD!

int schaarliftA = 3;

int schaarliftV = 11;

int knopmin = 12;

int knopmax = 13;

int m;

int n;

void setup()

{

  Serial.begin(9600);

  pinMode(schaarliftV, OUTPUT);

  pinMode(schaarliftA, OUTPUT);

  pinMode(knopmin, INPUT);

  pinMode(knopmax, INPUT);

  

}

void loop()

{

  do

  {

    analogWrite(schaarliftV, 255);

    analogWrite(schaarliftA, 0);

    n = digitalRead(knopmax);

    Serial.print("knopmax is ");

    Serial.print(n);

    Serial.println(" ");

  }

  while(n !=1);

  analogWrite(schaarliftV, 0);

  analogWrite(schaarliftA, 0);

  delay(5000);

  do

  {

    analogWrite(schaarliftA, 255);

    analogWrite(schaarliftV, 0);

    m = digitalRead(knopmin);

    Serial.print("knopmin is ");

    Serial.print(m);

    Serial.println(" ");

  }

  while(m != 0);

  analogWrite(schaarliftV, 0);

  analogWrite(schaarliftA, 0);

  delay(5000);

}

The new code for the scissor lift, working like a charm. The lift goes up till it hits the button, stops, waits a bit, then goes down to repeat the process.

//d1 hand test

#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 = A1;

int range = analogRead(A1);

int MotorV = 11;

int MotorA = 3;

int Flexsensor = A0;          //initiation hand

int Ventiel2open = 8;

int Ventiel2dicht = 9;

int flexfles = analogRead(A0);

int bladpos = 0;

int ks;

int ks1;

void setup()

{

  Serial.begin(9600);

  

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

}

void loop()

{ 

  flexfles = analogRead(A0);

  Serial.println(flexfles);

  if(flexfles > 800 && S1OUT[2] == 0)

  {

    Serial.print(flexfles);

    digitalWrite(Ventiel2open, HIGH);

    delay(50);

    digitalWrite(Ventiel2open, LOW);

    S1OUT[2] = 1;

    bladpos = 1;

    S1OUT[3] = 1;

  }

  delay(5000);

   digitalWrite(Ventiel2dicht, HIGH);

   delay(50);

   digitalWrite(Ventiel2dicht, LOW);

  

}

    

 An updated code for the hand.

//Valve control test

int pin1 = 6;

int pin2 = 7;

int pin3 = 8;

int pin4 = 9;

int flex = A0;

int a;

int commando;

void setup()

{

  pinMode(pin1, OUTPUT);

  pinMode(pin2, OUTPUT);

  pinMode(pin3, OUTPUT);

  pinMode(pin4, OUTPUT);

  Serial.begin(9600);

}

void loop()

{

  a= analogRead(flex);

  Serial.print(a);

  commando=Serial.read() - '0';

    Serial.println(commando);

    

    switch(commando)

    {

      case 1:

      {

      digitalWrite(pin1, HIGH);

      delay(50);

      digitalWrite(pin1, LOW);

      break;

      }

      case 2:

      {

        digitalWrite(pin2, HIGH);

        delay(50);

        digitalWrite(pin2, LOW);

        break;

      }

      case 3:

      {

        digitalWrite(pin3, HIGH);

        delay(50);

        digitalWrite(pin3, LOW);

        break;

      }

      case 4:

      {

        digitalWrite(pin4, HIGH);

        delay(50);

        digitalWrite(pin4, LOW);

        break;

      }

      

}

}

A simple but effective code to test the valve control, by sending commands over the serial interface we could test quick and effective. 

However with all this testing and a lot of errors that had to be fixed in a "creative" manner we accidentally fried one of the arduino controllers, and after replacing it with a new one I2C started bugging.... After 2 days of almost 12 hours of work this was the first mayor problem we couldn't quite pinpoint. So Tommy was taken home again to fix all the last errors, with only 1 day to go before the race.... As it turned out different batches of arduino support different pieces of hardware, and we put 2 together that could not communicate.

The next day 8:00 Tommy was error free, at the cost of the night rest of my colleges. As we still had a lot to do we started testing like a bunch of madman. The focus was at line tracking, because Mark had had so much work on all the errors the code was never implemented in Tommy. And Tommy turned out to be a bit harder to control then a small RC car the code was tested on. Unable to test the master and p&p because of that I spend most my time supporting others and refining my code.

Then it was 21:00 and the school closed, Tommy was not quite done... Line tracking gasped the concept of what it had to do, but following a line accurate was out of the question. We decided to go home, and get some sleep, to start off 4:00 the next morning. 

Here are some pictures of Tommy the last week, as next post will cover the CCM trophy challenge and and all its drama. 

Subsystem test and System integration

When most of the sub-systems had taken shape, we were urged to put it all together. Entering this "final phase" where you start working on the total product in stead of smaller parts needed to happen soon because time was getting shorter... So as we decided, 17:28 Friday afternoon we were doing the first test with the shadow of our final product.
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, 

I had to add another status bit to the slave 1, to avoid the instance of picking up the bottle, and deciding to place it back in the same spot.

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.   
    
    
  

Putting the first pieces together, subsystem integration!

This is the phase where the first pieces start to take form, for me as coder of the bunch this meant 2 things, first I had some more time to work on my code, second as the sub systems are generally speaking more of materialistic beings, it also involved a lot of testing and error filtering.
My college Mark had his system box finished by this time, so we spend a great amount of time filtering all the errors, and we had quite some errors to find.... The first mayor error we found was one we could have known, what we fount where random signals from our sensors and buttons. After some investigation it turned out all 3 our arudino's where reading gibberish, all on 2 pins.... after some quick research we had wired those sensors to pin 1 and 0, the serial pins. And because we where using "Serial Read" to actually read the values it was no wonder it didn't make any sense. Luckily we had some spare pins, so after some surgical interventions by our Electra doctor Mark  we had most of the COM connections working in order. These COM's where only the first 6 wires coming out of the box tough, so we had quite a way to go. Next up where the transistors, luckily those worked like a charm, and our valves started ticking away just like we wanted them to. Then it got interesting again, the H-bridges, used to control our engines it was vital those would work. In the first test we had no problems, but when we did the first test with the scissor lift a sloppy mistake from my part fried the component.... This turned out to be easily replaced, but what wasn't as easy to fix was the scissor lift itself. As it turned out, the clearance on the scissors was to big, and the whole lift was unstable, as well as the transmission from the engine that produces way to much friction for the lift to be operated by the engine.
Driving my teammates insane and forcing them back to the drawing table was a bit of a setback, especial because we couldn't continue testing with the sub-systems.
Looking back at it now this was the first time Murphy came by to give us a little nod, to make sure whatever we where doing was done well. However we went on, and it would come back to bite us in the a**.

This however gave me some time to continue working on the code, so just to get a first version ready for testing I wrote Version 1.0 of Pick and Place.

//TEST 1.0 PICK AND PLACE SLAVE 1

#include <Servo.h>
Servo myservo;              //initiation arm
int Ventielopen = 8;
int Ventieldicht = 9;
int flescounter = 0;

int knopmax = 4;            //initiation schaarlift
int knopmin = 13;
int IRsensorpin = A1;
int range = analogRead(A1);

int Flexsensor = 3;          //initiation hand
int Ventiel2open = 7;
int Ventiel2dicht = 6;
int range2 = analogRead(A0);


int commando = 0;           //commando variable als vervanging I2C


void setup()
{
   Serial.begin(9600);       //setup arm
  myservo.attach(5);
  pinMode(Ventielopen, OUTPUT);
  pinMode(Ventieldicht, OUTPUT;
  
  pinMode(knopmax, INPUT);          //setup schaarlift
  pinMode(knopmin, INPUT);
  pinMode(IRsensorpin, INPUT);
  
  pinMode(Ventiel2open, OUTPUT);    //setup hand
  pinMode(Ventiel2dicht, OUTPUT);
  pinMode(Flexsensor, INPUT);
}

void loop()
{
  if(Serial.available()>0)  // Gerbuik de serial interface om I2C commando's te imiteren
  {
    commando=Serial.read() - '0';
    Serial.println(commando);
  }
  
  switch(commando)
  {
    case 1:  
    {
      void StartArm();    //Prepare Pick and Place
      void SchaarUp();
     
    break;
    }
    
    case 2:                 //Pak fles
     {
     void SchaarDown();
      if(range2 < 780)       //Getest en realistiche waarde
      {  
      int pakfles();
      }
     break;
     }
     
    case 3:               //prepare drop fles
    {
    void SchaarUp();
    int MoveArm(flescounter);
    
    break;
    }
    
    case 4:              //Zet fles 
    {
    void SchaarDown();
    if (flescounter==1 && range2 < 780)     //testen en juiste waarden vinden
    {
    int dropfles();
    }
        
    break;
  }
    case 5:
    {
      analogWrite(10, 0);
      analogWrite(11, 0);
 }
}
//-------------------------------------------------------------------------------------------------------------------------//
void SchaarUp()                //Schaarlift omhoog
{
  do
  {
    analogWrite(10, 192);
  }
  while(knopmax =!1);
}

void StartArm()                //Draai arm in positie
{  
  myservo.write(180);
  digitalWrite(Ventielopen, HIGH); //Schuif de arm uit
  delay(100);
  digitalWrite(Ventielopen, LOW);
}

void SchaarDown()                //Schaarlift omlaag
{            
  range = analogRead(A1);
  do
  {
    analogWrite(11, 192);
  }
  while(knopmin =! 1 || range < 450);    //getest 
}

int pakfles()                    //Pak de fles
{
  delay(10);
  digitalWrite(Ventiel2open, HIGH);
  digitalWrite(Ventiel2dicht, LOW);
  delay(100);                   //TESTEN en goede waarde vinden
  digitalWrite(Ventiel2open, LOW);
  
  int flescounter = 1;
  return flescounter;
}

int MoveArm(int counter)        //Draai arm in positie 2
{
  switch(counter)
  {
  case 0: myservo.write(180);
  break;
  
  case 1: myservo.write(0);
  break;
}
}

int dropfles()
{
  digitalWrite(Ventiel2open, LOW);
  digitalWrite(Ventiel2dicht, HIGH);
  delay(100);                    //testen en goede waarde vinden
  digitalWrite(ventiel2dicht, LOW);
  int flescounter = 0;
  return flescounter;
}
This version didn't incorporate the I2C wire library, in stead I used the Serial read (the proper way, as in reading what I send to the arduino) to imitate the commands the program would receive from the master.
It was a bummer however, that I could not test the program, simply put because the sub systems weren't ready. This program did however form the basis for the rest of the sub system, build on the template seen before.