Tuesday, 16 May 2017

Sprint 3 - Zhuo Fan Ye

Sprint overview
The pieces are coming together. The servo swing movements has been fixed, and is ready to be coded as a Real Time Operating System and test run on the actual lamp. This sprint is going to document the final processes up until demo day.


The unwanted swing movements
There were two types of unwanted movements that was reoccurring on a consistent basis. One of them was the slight jerking has been solved by using better and stronger servos that can handle torque; however there is one more type of jerking that isn’t the vibration type of jerking – the swing movement.

This swing movement happens occasionally, caused by the not-perfect synchronisation of the process of reading in data from the serial string. For example, when the string “F01X080Y080D123” is being sent in a specific time instance, the mbed receives this string halfway “F01X080Y080..” and then the next string is being sent in starts to override the previous string, resulting in “F01X080Y080F01X” instead of the intended “F01X080Y080D123”.

Acknowledgements were the solution to solving this issue. The python code was slightly modified to introduce a delay between each stream of string being sent, unless an acknowledgement is received, the subsequent string won’t be sent until the acknowledgement is received via the serial buffer. This feature fixed the swing movements and the servo movements are now ideal. Work was done with the Servo Team to optimise the code for it to operate like an RTOS.



The code

#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

/*
// new thread for serial interface with face team 
void serial_buffer(void const *args) 
        {   
        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);                    // 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
            //pc.printf("T=%d F=%d y=%d x=%d d=%d = \n",T,F,x y,d);
            
        if(x+85<170 amp="" x="">10) 
        {
            pan_degree = x+85;   // increase pan servo angle
            pc.printf("pandegree=%d \n",pan_degree );
            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(y+85<170 amp="" y="">10) 
        {
            tilt_degree = y+85;
            tilt_message_t *tilt_mes = mpool_tilt.alloc();
            tilt_mes->tilt_degree = tilt_degree;
            queue_tilt.put(tilt_mes);
        }
        else if(d<170 amp="" d="">10)
        {
            lean_degree = d;
            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");
        Thread::wait(tw);
        
}  
*/


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 2="" angle="" increase="" pan="" pan_degree="pan_degree" pan_mes-="" pan_mes="mpool_pan.alloc();" pan_message_t="" servo="">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 2="" tilt_degree="tilt_degree" tilt_mes-="" tilt_mes="mpool_tilt.alloc();" tilt_message_t="">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 amp="" if="" min="" pan_degree="10;" x="" y="">50)                 // CONTROL for Y
        {
            tilt_degree = y+85;                 // HAPPY region
            
        }
        else if(y+85>110)                       // MAX
        {
            tilt_degree = 110;    
        }
        else if(y+85<50 amp="" d="" if="" tilt_degree="50;">160)                      // D control for lean NEEDS WORK!!!!!!!!
        {
            lean_degree = 85;                   // HAPPY
        }
        else if(d<=160)
        {
            //pc.printf("Ditance 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(F == 1)
        {
        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 queu
        
        tilt_message_t *tilt_mes = mpool_tilt.alloc();
        tilt_mes->tilt_degree = tilt_degree;                // load degrees into tilt_mes
        queue_tilt.put(tilt_mes);                           // load degrees into Tilt queu
        
        lean_message_t *lean_mes = mpool_lean.alloc();
        lean_mes->lean_degree = lean_degree;                // load degrees into lean_mes
        queue_lean.put(lean_mes);                           // load degrees into lean queu
        }
        
        //pc.printf("Thread buffer end\n");
        }//turn off servo code
        Thread::wait(tw);
        }
      //  } // uncomment later when taking out "while buffer 16"
    
    }
}




No comments:

Post a Comment