Categories

Archive

Disclaimer

De meningen ge-uit door medewerkers en studenten van de TU Delft en de commentaren die zijn gegeven reflecteren niet perse de mening(en) van de TU Delft. De TU Delft is dan ook niet verantwoordelijk voor de inhoud van hetgeen op de TU Delft weblogs zichtbaar is. Wel vindt de TU Delft het belangrijk - en ook waarde toevoegend - dat medewerkers en studenten op deze, door de TU Delft gefaciliteerde, omgeving hun mening kunnen geven.

motor code

Today a program was written to regulate motor movements (of 2 servo motors). This piece of code will be tested tomorrow (Thursday 27th) and afterwards be merged with the existing code that uses the temperature sensor as input. Again, comments are in grey.

I realize there are only a few comments. This will be corrected later.

 

 

#include <Servo.h>

Servo servo1;
Servo servo2;

// constants
const int bound1 = 100;
const int bound2 = 150;
const int beginPos1 = 0;
const int beginPos2 = 100;

// global variables
int pos1 = beginPos1;
int pos2 = beginPos2;

void setup() {
   Serial.begin(9600);
   servo1.attach(9);
   servo2.attach(10);
}

void loop() {
   rePosition();
}

void rePosition() {
   if ((pos1 < bound1) && (pos2 < bound2)) { // move both servos
      motor(3,1);
      delay(50);
   }  
   if ((pos1 < bound1) && (pos2 >= bound1)) { // moves servo1
      motor(1,1);
      delay(50);
   }
   if ((pos2 < bound2) && (pos1 >= bound1)) { // moves servo2
      motor(2,1);
      delay(50);
   }
   if (((pos1 >= bound1) && (pos2 >= bound1)) && ((pos1 < bound2) && (pos2 < bound2))) { // moves both servos
      motor(3,1);
      delay(50);
   }
   if ((pos1 >= bound2) && (pos2 >= bound2)) { // back to initial state
      motor(4,0); 
      delay(1000);
   }
}

void motor(int c, int s) {
   // c determines the case for which servo should move
   // s determines the speed

   switch(c) {
      case 0:
         // nothing moves
      break;
      case 1:
         // servo1 moves
         pos1 = pos1 + s;
         servo1.write(pos1);
      break;
      case 2:
         // servo2 moves
         pos2 = pos2 + s;
         servo2.write(pos2);
         break;
      case 3:
         // servo1 and servo2 move
         pos1 = pos1 + s;
         pos2 = pos2 + s;
         servo1.write(pos1);
         servo2.write(pos2);
      break;
      case 4: 
         // set servo1 and servo2 back to initial state
         pos1 = beginPos1;
         pos2 = beginPos2;
         servo1.write(pos1);
         servo2.write(pos2);
      break;
      case 5:
         // wiggle movement servo 1
         // is not used in the program at the moment, but will be in the final program

         int g2 = pos1 + 10; // relative bounds (g1 and g2)
         int g1 = pos1;
         for (pos1; pos1 < g2; (pos1 += s)) {
            servo1.write(pos1);
            delay(50);
         }
         for (pos1; pos1 > g1; pos1 -= s) {  
            servo1.write(pos1);
            delay(50);
         }
      break;
   }
}

Be Sociable, Share!

Leave a Reply

© 2011 TU Delft