Monday, 15 May 2017

Sprint 3 - Vision Team - Deepak

Goal: To combine with the motor team to integrate the mbed code using RTOS and test using the mini-lampbot
Minutes of the Meeting:
Ø  Status on the Vision Systems Developed
o   Fully functional facial recognition (up to 6 faces & 300cm in distance) and object (green ball) detection by using toggle key.
o   Fully functional mini-lampbot, with small jitters that can be fixed when integrated with the motor team
Ø  Instructions from Client (Mr. Jason Berry)
o   Combine with motor team – work with Thomas Murphy to combine the serial buffer with existing RTOS system developed by motor team
o   Test the on the mini-lampbot and then on the big motors
Report on assigned work:
In this sprint we worked closely with the motor team and our initial task was to integrate the serial buffer in to an existing RTOS system designed by the motor team. My task specifically was to create a serial thread for the existing serial buffer that processes the data from the vision system. Initially a simple serial thread was created to integrate with the existing RTOS. But that didn’t work because the serial data was crucial in getting the data to drive the motors (X, Y and Z).  Hence, the serial buffer was directly included into the main thread of the RTOS.
The serial buffer that I received from terry is explained below:
Serial pc(USBTX, USBRX);
// Buffer
char *token;  //pointer
char buffer[128];
int f,d,x,y,yt;
Here we define the variables required
 

pc.baud(9600);
PC baud rate is set

while(1) {               
//pc.printf("Thread buffer\n");            
pc.gets(buffer,17); 
//char buffer[] = "T0F1x047y014d052";             
lcd.locate(0,0);            
lcd.printf("%s\n", buffer);            
token = strtok(buffer, " '' TFxyd");            
T = atoi(token);
token = strtok(NULL, " '' TFxyd");            
F = atoi(token);    // Get face #            
token = strtok(NULL, " '' TFxyd");           
x = atoi(token);            
pc.printf("%d", x);   // Det x pos            
token = strtok(NULL, "'' TFxyd");
y = atoi(token);   // Get y Pos
token = strtok(NULL, "'' TFxyd");
d = atoi(token);   // Get D pos
lcd.locate(0,21);
//lcd.locate(0,17);
yt = y;   // Set new vari for d outside sweetspot
lcd.printf("y=%d x=%d D=%d",x, y,d);
·       Converts the string argument ‘str’ to an integer (type ‘int’) is what is done in this section of the code
·       Next the string is split up and we use ‘atoi’ to convert them to individual integer values.

After combining with Motor team the fully executable RTOS code is shown below:


#include "mbed.h"
#include "Servo.h"
#include "rtos.h"
#include "C12832.h"
#include "string"
#include "cstring"

#define SERVO_PIN p21
#define SERVO2_PIN p22
#define SERVO3_PIN p23

Serial pc(USBTX, USBRX); // tx, rx
Servo servo_pan(SERVO_PIN);
Servo servo_tilt(SERVO2_PIN);
Servo servo_lean(SERVO3_PIN);
BusIn joy(p15,p12,p13,p16);

bool debug = 1;

C12832 lcd(p5, p7, p6, p8, p11);

char *token;
char buffer[128];
int T,F,d,x,y,yt;

int pan_degree = 85;
int tilt_degree = 85;
int lean_degree = 85;


int tw = 100;            // thread wait value

typedef struct {
    int pan_degree;   // Pan servo angle
} pan_message_t;

typedef struct {
    int tilt_degree;   // Tilt servo angle
} tilt_message_t;

typedef struct {
    int lean_degree;   // lean servo angle
} lean_message_t;

MemoryPool mpool_pan; // memory pool for pan servo
Queue queue_pan;      // queue for pan servo

MemoryPool mpool_tilt; // memory pool for tilt servo
Queue queue_tilt;      // queue for tilt servo

MemoryPool mpool_lean; // memory pool for lean servo
Queue queue_lean;      // queue for lean servo


void lean_servo(void const *args) { // lean servo thread
     while (true)
    {
        //pc.printf("Thread Lean start\n");
        osEvent evt = queue_lean.get();
        if (evt.status == osEventMessage)
        {
            lean_message_t *lean_mes = (lean_message_t*)evt.value.p;
            servo_lean.write(lean_mes->lean_degree);
           // pc.printf("Thread Lean mess = %d \n", &lean_mes);
            pc.printf("Thread Lean deg = %d \n", lean_degree);
            mpool_lean.free(lean_mes);
        } 
        //pc.printf("Thread Lean end\n");
        Thread::wait(tw);
    }
}


void pan_servo(void const *args)
{
    while (true)
    {
        //pc.printf("Thread pan start\n");
        osEvent evt = queue_pan.get();
        if (evt.status == osEventMessage)
        {
            pan_message_t *pan_mes = (pan_message_t*)evt.value.p;
            servo_pan.write(pan_mes->pan_degree);
            //pc.printf("Thread pan mess = %d \n", &pan_mes);
             pc.printf("Thread pan deg = %d \n", pan_degree);
            mpool_pan.free(pan_mes);
        }
        //pc.printf("Thread pan end\n"); 
        Thread::wait(tw);
    }
}

void tilt_servo(void const *args)
{
    while (true)
    {
        //pc.printf("Thread tilt start\n");
        osEvent evt = queue_tilt.get();
        if (evt.status == osEventMessage)
        {
            tilt_message_t *tilt_mes = (tilt_message_t*)evt.value.p;
            servo_tilt.write(tilt_mes->tilt_degree);
            //pc.printf("Thread tilt mess = %d \n", &tilt_mes);
             pc.printf("Thread tilt deg = %d \n", tilt_degree);
            mpool_tilt.free(tilt_mes);
        }
        //pc.printf("Thread tilt end\n");
        Thread::wait(tw);
    }
}

int main(void const *args) {
    Thread thread1(lean_servo);
    Thread thread2(pan_servo);
     Thread thread3(tilt_servo);
   
    //serial read variables
    char inByte = 0;
    int inCNT = 0;
   
    pc.baud(9600);
    lcd.cls();
    pc.printf(" start\n");
    servo_pan.write(85); // inital position
    servo_tilt.write(85); // inital position
    servo_lean.write(85); // inital position
   
    //Thread thread(serial_buffer);
    pc.printf("Thread 4 \n");
    // up =4, down = 8, right = 1, left = 2 - joystick direction values
    if(debug)
    {
        while (true)
        {
            if(joy ==1 && pan_degree<155 o:p="">
            {
                pan_degree = pan_degree + 2;    // increase pan servo angle
                pan_message_t *pan_mes = mpool_pan.alloc();
                pan_mes->;pan_degree=pan_degree;  // load degrees into pan_mes
                queue_pan.put(pan_mes); // load degrees into pan queue
            }
            else if(joy ==2 && pan_degree>50)
            {
                pan_degree = pan_degree - 2;    // decrease pan servo angle
                pan_message_t *pan_mes = mpool_pan.alloc();
                pan_mes->pan_degree = pan_degree;   
                queue_pan.put(pan_mes);
            }
            if(joy ==1 && tilt_degree<110 o:p="">
            {
                tilt_degree = tilt_degree + 2;
                tilt_message_t *tilt_mes = mpool_tilt.alloc();
                tilt_mes->tilt_degree = tilt_degree;
                queue_tilt.put(tilt_mes);
            }
            else if(joy == 8 && tilt_degree>10)
            {
                tilt_degree = tilt_degree - 2;
                tilt_message_t *tilt_mes = mpool_tilt.alloc();
                tilt_mes->tilt_degree = tilt_degree;
                queue_tilt.put(tilt_mes);
            }
            Thread::wait(tw);
        }
    }
    else
    {
            while(true)
            {
                inByte = pc.getc();   
                if (1)//(inByte == 'S')
                    {
                   // inByte = pc.getc();
                    //pc.printf("Thread buffer\n");           
                    pc.gets(buffer,17);
                    //char null[] = "0";
                    //strcat(buffer,null);
                    /*  while(buffer[16] != '\n')
                    {*/
                    //char buffer[] = "T0F1x047y014d052"; 
                    //lcd.cls();
                    //lcd.locate(0,0);
                    //lcd.printf("%s\n", buffer);
                    //pc.printf("1");
                    token = strtok(buffer, " '' TFxyd");
                    T = atoi(token);
                    token = strtok(NULL, " '' TFxyd");
                    F = atoi(token);                    // Get face #
                    token = strtok(NULL, " '' TFxyd");
                    x = atoi(token);
                   // pc.printf("%d", x);                    // Det x pos
                    token = strtok(NULL, "'' TFxyd");
                    y = atoi(token);                    // Get y Pos
                    token = strtok(NULL, "'' TFxyd");
                    d = atoi(token);                    // Get D pos
                    //lcd.locate(0,21);
                    //lcd.locate(0,17);
                    //yt = y;           // Set new vari for d outside sweetspot
                    //lcd.printf("y=%d x=%d D=%d",x, y,d);
                     pc.printf("1");
                    }//end of start char found
        //pc.printf("1");
        if(1)//turn on servo code
        {
                    if(x+85<170 amp="" x="">10)                 // Control for x                    {
                    pan_degree = x+85;                  // HAPPY region
            //pc.printf(" = %d \n", lean_degree);   
        }
        else if(x+85>170)
        {
            pan_degree = 170;                   // MAX
        }
        else if(x+85<10 10="" o:p="">
        {
            pan_degree = 10;                    // MIN
        }
       
        if(y+85<110 amp="" x=""><110 amp="" x="">50)                 // CONTROL for Y
        {
            tilt_degree = y+85;                 // HAPPY region
           
        }
        else if(y+85>110)                       // MAX
        {
            tilt_degree = 110;   
        }
        else if(y+85<50 50="" o:p="">
        {
            tilt_degree = 50;                   //MIN
        }

        if(d<180 amp="" d="">160)                      // D control for lean
        {
            lean_degree = 85;                   // HAPPY
        }
        else if(d<=160)
        {
            //pc.printf("Distance should move back\n HOW FAR?????? 60");
            //tilt_degree = tilt_degree - 2;
            lean_degree = 110;   
        }
        else if(d = 180)
        {
            //pc.printf("Ditance should move forward\n HOW FAR??????");
            //tilt_degree = tilt_degree - 2;   
            lean_degree = 60;
        }
        //pc.printf("pandegree=%d \n",pan_degree );
        //pc.printf("tiltdegree=%d \n",tilt_degree );
        //pc.printf("leanegree=%d \n",lean_degree );
        //pc.printf("pan mess = %d \n", pan_mess);
       
        if (T == 1 || F == 1)           //Object or Face 1 Detected
                            {
                                //pc.printf("pandegree=%d \n",pan_degree );
                                //pc.printf("tiltdegree=%d \n",tilt_degree );
                                //pc.printf("leanegree=%d \n",lean_degree );
                                //pc.printf("pan mess = %d \n", pan_mess);
                                pan_message_t *pan_mes = mpool_pan.alloc();
                                pan_mes>pan_degree=pan_degree;                                                    queue_pan.put(pan_mes);
                                tilt_message_t *tilt_mes = mpool_tilt.alloc();
                                tilt_mes->tilt_degree = tilt_degree;                                           queue_tilt.put(tilt_mes);   
                                lean_message_t *lean_mes = mpool_lean.alloc();
                                lean_mes->lean_degree = lean_degree; 
                                queue_lean.put(lean_mes);       
                            }
        //pc.printf("Thread buffer end\n");
        }//turn off servo code
        Thread::wait(tw);
        }
      //  } // uncomment later when taking out "while buffer 16"
   
    }
}

Challenges:
The next step was to test the code on the mini-lampbot, and when tested the worked absolutely fine expect when it reached the extreme limits on vision system, we would see crazy oscillations on the Y and Z motors. We tried multiple things to fix it and was in vain, but when we switched motors Y and Z it didn’t do that. Hence, we assumed it could be because of the load on the servo motors could be causing this. Keeping that knowledge in mind we tested the same on the big motors that was going to be used in actual project and it just worked perfectly fine and we realized that the code worked absolutely fine and it was the issues with the motors that caused the crazy oscillations. Hence the given task for sprint 3 was complete and we were ready for final testing on the actual lamp.

Summary of Sprint 3:
Ø  Test the Mini Lampbot
o   Vision team to work with servo team to combine code
o   Test the combined code on the mini lampbot
o   Test on the big servos that are actually used


No comments:

Post a Comment