Monday, 16 March 2015

Sprint #2: Servo Motion - David Araújo and Hervé Luyindula

Objective: describes the implementation of the serial communication protocol (pc to mbed) to generate the movement of a 2-axis lamp.

Overview: The motion is controlled from the serial data transmitted from Hyperterminal sending an integer ranging from 0 to 8. As the data is received, it is processed via the thread intelligence to distinguish the nature of the data. The servos' motion is executed according the data received.


Features: 
  • Servo Rotate Thread (x-axis motion)
  • Servo Tilt Thread (y-axis motion)
  • Intelligence Thread
  • Local Indicator (LCD)
  • Mutex lock
  • PC to serial (rs232)
Video:



Code:

#include "mbed.h"
#include "USBHostSerial.h"
#include "C12832_lcd.h"
#include "Servo.h"

#define  RTIME         if(posx<=1500)  posx+=40;
#define  RCOUNTERTIME   if(posx>=500) posx-=40;
#define TILTBACK       if(posy>=500)   posy-=40;
#define TILTFORW        if(posy<=1300)  posy+=40;
#define TEMPORARY       rotate.SetPosition(posx); tilt.SetPosition(posy);

DigitalOut led1(LED1);
DigitalOut led3(LED3);
Serial pc(USBTX, USBRX);
C12832_LCD lcd;

Servo tilt(p21);
Servo rotate(p22);      
AnalogIn ainLeft(p15);
AnalogIn ainRight(p16);

Mutex rx_mutex;
Mutex tiltserv;
Mutex rotateserv;

int posx = 1000,posy = 750;

void rotate_servo(int x);

void tild_thread(void const *args) 
{
    while (true) {
       tiltserv.lock();
       tilt.SetPosition(posy);
       tiltserv.unlock();
                }
}
                
void rotate_thread(void const *args) 
{
    while (true) {
        rotateserv.lock();
        rotate.SetPosition(posx);
        rotateserv.unlock();
                }
}

int main()
{
    tilt.Enable(750,20000);
    rotate.Enable(1000,20000);
    Thread thr1(tild_thread);
    Thread thr2(rotate_thread);
    lcd.cls();
    char buffer[2];
    rotate.SetPosition(1000);
    tilt.SetPosition(750);
   
    while (true)
    {
        if(pc.readable()) 
        {
            rx_mutex.lock();
            pc.gets(buffer,2); // set the number of bytes
            
            rx_mutex.unlock();
            lcd.locate(0,1);
            lcd.printf(buffer);
        }
        int x=atoi(buffer);

        rotate_servo(x);
        
        for(int i = 0; i < 2; i++){
          buffer[i-1] = 0;   
        }
        //buffer = [];
    } 
}



void rotate_servo(int x){
    switch (x)
        {
            case 0:
                lcd.cls();
                lcd.locate(0,1);
                lcd.printf("Do nothing");
    
            break;
            
            case 1:
                lcd.cls();
                lcd.locate(0,1);
                lcd.printf("Right");
                RCOUNTERTIME;
         
            break;
            
            case 2:
                lcd.cls();
                lcd.locate(0,1);
                lcd.printf("down & right");
                RCOUNTERTIME;
                TILTFORW;
            
            break;
            
            case 3:
                lcd.cls();
                lcd.locate(0,1);
                lcd.printf("down");
                TILTFORW;
                             
            break;
            
            case 4:
                lcd.cls();
                lcd.locate(0,1);
                lcd.printf("Down & left");
                RTIME;
                TILTFORW;
                          
            break;
            
            case 5:
                lcd.cls();
                lcd.locate(0,1);
                lcd.printf("left");
                RTIME;
                

                
            break;
            
            case 6:
                lcd.cls();
                lcd.locate(0,1);
                lcd.printf("up & left");
                RTIME;
                TILTBACK;
                

                
            break;
            
            case 7:
                lcd.cls();
                lcd.locate(0,1);
                lcd.printf("up");
                TILTBACK;
               

                
            break;
            
            case 8:
                lcd.cls();
                lcd.locate(0,1);
                lcd.printf("up & right");
                TILTBACK;
                RCOUNTERTIME;
           
                
            break;
            
        }
    
    }




No comments:

Post a Comment