Revision 4:45bd9ba6c0da, committed 2018-05-24

Comitter:

ha731548874

Date:

Thu May 24 08:29:52 2018 +0000

Parent:

3:1345f959c490

Commit message:

NYP_humaniod_FYP

Changed in this revision

diff -r 1345f959c490 -r 45bd9ba6c0da kondo1.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kondo1.cpp	Thu May 24 08:29:52 2018 +0000
@@ -0,0 +1,118 @@
+#include "mbed.h"
+#include "kondo1.h"
+#include "servo.h"
+#define KONDO1_COMMAND_POSITION_FREE 1
+#define KONDO1_COMMAND_POSITION_SET 2
+#define KONDO1_STEP_PER_DEGREE 8000/270
+RawSerial KONDO1_serial(PC_4,PC_5);
+unsigned char KONDO1_Buffer_Tx[3];
+unsigned char KONDO1_Buffer_Tx_Ptr = 0;
+unsigned char KONDO1_Buffer_Tx_Full = false;
+unsigned char KONDO1_Buffer_Rx[6];
+unsigned char KONDO1_Buffer_Rx_Ptr = 3;
+unsigned char KONDO1_Buffer_Rx_Full = false;
+signed short int KONDO1_Positions_Initial[] = {7500,7500,7500,7500,7500,7500,7500};
+signed short int KONDO1_Positions_Target[KONDO1_MAX_TOTAL];
+//signed short int KONDO1_Positions_Current[KONDO1_MAX_TOTAL]= {7500,7500,7500,7500,7500};
+unsigned char KONDO1_SendOrder[]= {1,2,3,4,5,6};
+signed char KONDO1_Directions = 1;
+signed char KONDO1_ID = 0;
+signed char KONDO1_PtrCommands_Set;
+EventQueue KONDO1_queue;
+/*signed short int KONDO1_Position_Convert(char hi, char lo)
+{
+    signed short int i = hi;
+    i = i<<7;
+    i = i|lo;
+    return i;
+}
+void KONDO1_Position_Current_Set()
+{
+    unsigned char id;
+    id = KONDO1_Buffer_Rx[0]&0x1F;
+    KONDO1_Positions_Current[id] = KONDO1_Position_Convert(KONDO1_Buffer_Rx[4], KONDO1_Buffer_Rx[5]);
+}*/
+void KONDO1_Positions_Set(double* KONDO1_Degrees)
+{
+    unsigned char id;
+    for(KONDO1_ID=0;KONDO1_ID < sizeof(KONDO1_SendOrder);KONDO1_ID++)
+    {
+        id = KONDO1_SendOrder[KONDO1_ID];
+//            switch(KONDO1_PtrCommands_Set)
+//            {
+//                case KONDO1_COMMAND_POSITION_FREE:
+//                    KONDO1_Positions[id]=0;
+//                    break;
+//                case KONDO1_COMMAND_POSITION_SET:
+        KONDO1_Positions_Target[id] = KONDO1_Degrees[id] * KONDO1_STEP_PER_DEGREE * KONDO1_Directions + KONDO1_Positions_Initial[id];
+//                    break;
+//                  case KONDO1_COMMAND_DEGREE_SET:
+//                    KONDO1_degrees_Target[id] = (KONDO1_Positions_Target[id] - KONDO1_Positions_Initial[id])/ (KONDO1_STEP_PER_DEGREE * KONDO1_Directions);
+//                    break;
+//wait(0.000625);
+    }
+}
+void KONDO1_Interrupt_Tx()
+{
+    while(KONDO1_serial.writeable())
+    {
+        KONDO1_serial.putc(KONDO1_Buffer_Tx[KONDO1_Buffer_Tx_Ptr]);
+        KONDO1_Buffer_Tx_Ptr++;
+        if(KONDO1_Buffer_Tx_Ptr==3)
+              KONDO1_Buffer_Tx_Ptr = 0;
+    }
+}
+void KONDO1_Interrupt_Rx()
+{
+    while(KONDO1_serial.readable())
+    {
+        KONDO1_Buffer_Rx[KONDO1_Buffer_Rx_Ptr] = KONDO1_serial.getc();
+        KONDO1_Buffer_Rx_Ptr++;
+        if(KONDO1_Buffer_Rx_Ptr==6)
+        {
+            KONDO1_Buffer_Rx_Full = true;
+            KONDO1_Buffer_Rx_Ptr=0;
+//            KONDO1_Position_Current_Set();
+        }
+    }
+} 
+void KONDO1_update(unsigned char Id, unsigned short int Position)
+{
+    unsigned char id,lo,hi;
+    id=0x80|Id;
+    hi=(Position>>7)&0x007F;
+    lo=Position&0x007F;
+    NVIC_DisableIRQ(USART3_IRQn);
+    KONDO1_Buffer_Tx[0] = id;// KONDO1_Buffer_Rx[0] = KONDO1_serial.getc();
+    KONDO1_Buffer_Tx[1] = hi;// KONDO1_Buffer_Rx[1] = KONDO1_serial.getc();
+    KONDO1_Buffer_Tx[2] = lo;// KONDO1_Buffer_Rx[2] = KONDO1_serial.getc();
+//    wait(0.0007);      
+    KONDO1_Buffer_Rx_Full = false;
+    NVIC_EnableIRQ(USART3_IRQn);
+}
+void KONDO1_task() 
+{
+    signed char id;
+    signed char i;
+    for(i=0 ; i < sizeof(KONDO1_SendOrder) ; i++)
+    {
+        id = KONDO1_SendOrder[i];
+//        if(id==0)KONDO1_update(id, 0);else
+        KONDO1_update(id, KONDO1_Positions_Target[id]);
+        wait_us(650);    //650   1000
+//        printf("%d=%d\r\n",id,KONDO1_Positions_Current[id]);
+//        printf("%x,%x,%x,%x,%x,%x\r\n",KONDO1_Buffer_Rx[0],KONDO1_Buffer_Rx[1],KONDO1_Buffer_Rx[2],KONDO1_Buffer_Rx[3],KONDO1_Buffer_Rx[4],KONDO1_Buffer_Rx[5]);
+    }
+}
+void KONDO1_init()
+{
+    KONDO1_serial.format(8,Serial::Even,1);
+    KONDO1_serial.baud(115200);
+    KONDO1_Buffer_Rx_Ptr = 0;
+    KONDO1_Buffer_Rx_Full = false;
+    KONDO1_serial.attach(&KONDO1_Interrupt_Rx, Serial::RxIrq);
+    KONDO1_serial.attach(&KONDO1_Interrupt_Tx, Serial::TxIrq);
+    NVIC_EnableIRQ(USART3_IRQn);
+    KONDO1_queue.call_every(10, KONDO1_task);   //1000    0.1  1
+    KONDO1_queue.dispatch();
+}
\ No newline at end of file

diff -r 1345f959c490 -r 45bd9ba6c0da kondo1.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kondo1.h	Thu May 24 08:29:52 2018 +0000
@@ -0,0 +1,13 @@
+#ifndef __KONDO1_H__
+#define __KONDO1_H__
+
+#define KONDO1_MAX_TOTAL 32
+
+void KONDO1_Interrupt_Tx();
+void KONDO1_Interrupt_Rx();
+void KONDO1_update(unsigned char Id, unsigned short int Position);
+void KONDO1_task();
+void KONDO1_init();
+void KONDO1_Positions_Set(double* KONDO1_Degrees);
+
+#endif
\ No newline at end of file

diff -r 1345f959c490 -r 45bd9ba6c0da kondo2.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kondo2.cpp	Thu May 24 08:29:52 2018 +0000
@@ -0,0 +1,96 @@
+#include "mbed.h"
+#include "kondo2.h"
+#include "servo.h"
+#define KONDO2_COMMAND_POSITION_FREE 1
+#define KONDO2_COMMAND_POSITION_SET 2
+#define KONDO2_STEP_PER_DEGREE 8000/270
+RawSerial KONDO2_serial(PA_2,PA_3);
+unsigned char KONDO2_Buffer_Tx[3];
+unsigned char KONDO2_Buffer_Tx_Ptr = 0;
+unsigned char KONDO2_Buffer_Tx_Full = false;
+unsigned char KONDO2_Buffer_Rx[6];
+unsigned char KONDO2_Buffer_Rx_Ptr = 0;
+unsigned char KONDO2_Buffer_Rx_Full = false;
+signed short int KONDO2_Positions_Initial[] = {7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500};
+signed short int KONDO2_Positions_Current[KONDO2_MAX_TOTAL];
+signed short int KONDO2_Positions_Target[KONDO2_MAX_TOTAL];
+unsigned char KONDO2_SendOrder[]= {7,9,10};
+signed char KONDO2_Directions = 1;
+signed char KONDO2_PtrCommands_Set;
+signed char KONDO2_ID = 0;
+EventQueue KONDO2_queue;
+void KONDO2_Positions_Set(double* KONDO2_Degrees)
+{
+        unsigned char id;
+        for(KONDO2_ID=0;KONDO2_ID < sizeof(KONDO2_SendOrder);KONDO2_ID++)
+        {
+        id = KONDO2_SendOrder[KONDO2_ID];
+        KONDO2_Positions_Target[id] = KONDO2_Degrees[id] * KONDO2_STEP_PER_DEGREE * KONDO2_Directions + KONDO2_Positions_Initial[id];
+//        wait(0.0008);
+        }
+}
+void KONDO2_Interrupt_Tx() 
+{
+    while(KONDO2_serial.writeable())
+    {
+        KONDO2_serial.putc(KONDO2_Buffer_Tx[KONDO2_Buffer_Tx_Ptr]);
+//        KONDO2_Buffer_Rx[KONDO2_Buffer_Rx_Ptr] = KONDO2_serial.getc();
+        KONDO2_Buffer_Tx_Ptr++;
+ //       KONDO2_Buffer_Rx_Ptr++;
+        if(KONDO2_Buffer_Tx_Ptr==3 && KONDO2_Buffer_Rx_Ptr==3)
+              KONDO2_Buffer_Tx_Ptr = 0;
+//              KONDO2_Buffer_Rx_Ptr = 0;
+    }
+}
+void KONDO2_Interrupt_Rx()
+{
+    while(KONDO2_serial.readable())
+    {
+ //       KONDO2_Buffer_Rx_Ptr = 3;
+        KONDO2_Buffer_Rx[KONDO2_Buffer_Rx_Ptr] = KONDO2_serial.getc();
+        KONDO2_Buffer_Rx_Ptr++;
+        if(KONDO2_Buffer_Rx_Ptr==6)
+        {
+            KONDO2_Buffer_Rx_Full = true;
+            KONDO2_Buffer_Rx_Ptr=0;
+        }
+    }
+} 
+void KONDO2_update(unsigned char Id, unsigned short int Position)
+{
+    unsigned char id,lo,hi;
+    id=0x80|Id;
+    hi=(Position>>7)&0x007F;
+    lo=Position&0x007F;
+    NVIC_DisableIRQ(USART2_IRQn);
+    KONDO2_Buffer_Tx[0] = id;
+    KONDO2_Buffer_Tx[1] = hi;
+    KONDO2_Buffer_Tx[2] = lo;
+//    wait(0.0008);
+    KONDO2_Buffer_Rx_Full = false;
+    NVIC_EnableIRQ(USART2_IRQn);
+}
+void KONDO2_task() 
+{
+    signed char id;
+    signed char i;
+    for(i=0 ; i < sizeof(KONDO2_SendOrder) ; i++)
+    {
+        id = KONDO2_SendOrder[i];
+        KONDO2_update(id, KONDO2_Positions_Target[id]);
+//        wait_us(825);
+        wait_us(650);
+    }
+}
+void KONDO2_init() 
+{
+    KONDO2_serial.format(8,Serial::Even,1);
+    KONDO2_serial.baud(115200);
+    KONDO2_Buffer_Rx_Ptr = 0;
+    KONDO2_Buffer_Rx_Full = false;
+    KONDO2_serial.attach(&KONDO2_Interrupt_Rx, Serial::RxIrq);
+    KONDO2_serial.attach(&KONDO2_Interrupt_Tx, Serial::TxIrq);
+    NVIC_EnableIRQ(USART2_IRQn);    
+    KONDO2_queue.call_every(10, KONDO2_task);
+    KONDO2_queue.dispatch();
+}

diff -r 1345f959c490 -r 45bd9ba6c0da kondo2.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kondo2.h	Thu May 24 08:29:52 2018 +0000
@@ -0,0 +1,13 @@
+#ifndef __KONDO2_H__
+#define __KONDO2_H__
+
+#define KONDO2_MAX_TOTAL 32
+
+void KONDO2_Interrupt_Tx();
+void KONDO2_Interrupt_Rx();
+void KONDO2_update(unsigned char Id, unsigned short int Position);
+void KONDO2_task();
+void KONDO2_init();
+void KONDO2_Positions_Set(double* KONDO2_Degrees);
+
+#endif
\ No newline at end of file

diff -r 1345f959c490 -r 45bd9ba6c0da kondo3.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kondo3.cpp	Thu May 24 08:29:52 2018 +0000
@@ -0,0 +1,98 @@
+#include "mbed.h"
+#include "kondo3.h"
+#include "servo.h"
+#define KONDO3_COMMAND_POSITION_FREE 1
+#define KONDO3_COMMAND_POSITION_SET 2
+#define KONDO3_STEP_PER_DEGREE 8000/270
+RawSerial KONDO3_serial(PA_0,PA_1);
+unsigned char KONDO3_Buffer_Tx[3];
+unsigned char KONDO3_Buffer_Tx_Ptr = 0;
+unsigned char KONDO3_Buffer_Tx_Full = false;
+unsigned char KONDO3_Buffer_Rx[6];
+unsigned char KONDO3_Buffer_Rx_Ptr = 0;
+unsigned char KONDO3_Buffer_Rx_Full = false;
+signed short int KONDO3_Positions_Initial[] = {7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500};
+signed short int KONDO3_Positions_Current[KONDO3_MAX_TOTAL];
+signed short int KONDO3_Positions_Target[KONDO3_MAX_TOTAL];
+unsigned char KONDO3_SendOrder[]= {16,15,14,13,12,11};
+signed char KONDO3_Directions = 1;
+signed char KONDO3_ID = 0;
+signed char KONDO3_PtrCommands_Set;
+EventQueue KONDO3_queue;
+void KONDO3_Positions_Set(double* KONDO3_Degrees)
+{
+    unsigned char id;
+    for(KONDO3_ID=0;KONDO3_ID < sizeof(KONDO3_SendOrder);KONDO3_ID++)
+    {
+    id = KONDO3_SendOrder[KONDO3_ID];
+//            switch(KONDO1_PtrCommands_Set)
+//            {
+//                case KONDO1_COMMAND_POSITION_FREE:
+//                    KONDO1_Positions[id]=0;
+//                    break;
+//                KONDO1_COMMAND_POSITION_SET:
+    KONDO3_Positions_Target[id] = KONDO3_Degrees[id] * KONDO3_STEP_PER_DEGREE * KONDO3_Directions + KONDO3_Positions_Initial[id];
+//    wait(0.0008);
+    }  
+}
+void KONDO3_Interrupt_Tx() 
+{
+    while(KONDO3_serial.writeable())
+    {
+        KONDO3_serial.putc(KONDO3_Buffer_Tx[KONDO3_Buffer_Tx_Ptr]);
+        KONDO3_Buffer_Tx_Ptr++;
+        if(KONDO3_Buffer_Tx_Ptr==3)
+              KONDO3_Buffer_Tx_Ptr = 0;
+    }
+}
+void KONDO3_Interrupt_Rx()
+{
+    while(KONDO3_serial.readable())
+    {
+        KONDO3_Buffer_Rx[KONDO3_Buffer_Rx_Ptr] = KONDO3_serial.getc();
+        KONDO3_Buffer_Rx_Ptr++;
+        if(KONDO3_Buffer_Rx_Ptr==6)
+        {
+            KONDO3_Buffer_Rx_Full = true;
+            KONDO3_Buffer_Rx_Ptr=0;
+        }
+    }
+} 
+void KONDO3_update(unsigned char Id, unsigned short int Position)
+{
+    unsigned char id,lo,hi;
+    id=0x80|Id;
+    hi=(Position>>7)&0x007F;
+    lo=Position&0x007F;
+    NVIC_DisableIRQ(USART1_IRQn);
+    KONDO3_Buffer_Tx[0] = id;
+    KONDO3_Buffer_Tx[1] = hi;
+    KONDO3_Buffer_Tx[2] = lo;
+//    wait(0.0008);
+    KONDO3_Buffer_Rx_Full = false;
+    NVIC_EnableIRQ(USART1_IRQn);
+}
+void KONDO3_task() 
+{
+    signed char id;
+    signed char i;
+    for(i=0 ; i < sizeof(KONDO3_SendOrder) ; i++)
+    {
+        id = KONDO3_SendOrder[i];
+        KONDO3_update(id, KONDO3_Positions_Target[id]);
+        wait_us(650);
+//        wait_us(825);
+    }
+}
+void KONDO3_init() 
+{
+    KONDO3_serial.format(8,Serial::Even,1);
+    KONDO3_serial.baud(115200);
+    KONDO3_Buffer_Rx_Ptr = 0;
+    KONDO3_Buffer_Rx_Full = false;
+    KONDO3_serial.attach(&KONDO3_Interrupt_Rx, Serial::RxIrq);
+    KONDO3_serial.attach(&KONDO3_Interrupt_Tx, Serial::TxIrq);
+    NVIC_EnableIRQ(USART1_IRQn);
+    KONDO3_queue.call_every(10, KONDO3_task); //1000
+    KONDO3_queue.dispatch();
+}
\ No newline at end of file

diff -r 1345f959c490 -r 45bd9ba6c0da kondo3.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kondo3.h	Thu May 24 08:29:52 2018 +0000
@@ -0,0 +1,13 @@
+#ifndef __KONDO3_H__
+#define __KONDO3_H__
+
+#define KONDO3_MAX_TOTAL 32
+
+void KONDO3_Interrupt_Tx();
+void KONDO3_Interrupt_Rx();
+void KONDO3_update(unsigned char Id, unsigned short int Position);
+void KONDO3_task();
+void KONDO3_init();
+void KONDO3_Positions_Set(double* KONDO3_Degrees);
+
+#endif
\ No newline at end of file

diff -r 1345f959c490 -r 45bd9ba6c0da main.cpp
--- a/main.cpp	Wed May 02 07:49:13 2018 +0000
+++ b/main.cpp	Thu May 24 08:29:52 2018 +0000
@@ -1,38 +1,32 @@
 #include "mbed.h"
 #include "rtos.h"
+//#include "kondo1.h"
+#include "kondo2.h"
+//#include "kondo3.h"
 #include "servo.h"
 #include "accgyro.h"
-//Serial pc(USBTX, USBRX);
 DigitalOut led2(LED2);
 Thread event_thread;
 Thread ACCGYRO_thread;
-Thread SERVO_thread;
+//Thread KONDO1_thread;
+Thread KONDO2_thread;
+//Thread KONDO3_thread;
+//Thread KONDO1_Degrees_Set_thread;
+Thread KONDO2_Degrees_Set_thread;
+//Thread KONDO3_Degrees_Set_thread;
 EventQueue event_queue;
-
 int main()
 {   
     ACCGYRO_thread.start(ACCGYRO_init);
-    SERVO_thread.start(SERVO_init);
-    //SERVO_init();
-    //event_thread.start(callback(&event_queue, &EventQueue::dispatch_forever));
-    //ACCGYRO_init();
-    event_thread.start(callback(&event_queue, &EventQueue::dispatch_forever));
-    //Thread::wait(osWaitForever);
-    event_queue.call_every(1000, SERVO_task);
-    event_queue.call_every(1000, ACCGYRO_task);
-    //q.dispatch();
-    //eventqueue.call_every(1000,printf,"LSM6DSL [acc/mg]:        %6ld\r\n", ACCGYRO_get_gyro_x());
-    //q.dispatch();
+//    KONDO1_thread.start(KONDO1_init);
+    KONDO2_thread.start(KONDO2_init);
+//    KONDO3_thread.start(KONDO3_init);
+//    KONDO1_Degrees_Set_thread.start(KONDO1_Degrees_Set);
+    KONDO2_Degrees_Set_thread.start(KONDO2_Degrees_Set);
+//    KONDO3_Degrees_Set_thread.start(KONDO3_Degrees_Set);
     while(1)
     {
-        //eventqueue.call(printf,"LSM6DSL [acc/mg]:        %6ld\r\n", ACCGYRO_get_gyro_x());
         led2 = !led2;
         wait(1);
-        //q.dispatch();
-        //q.call(printf, "*\n"); 
-        //printf("*");
-        //Thread::wait(1000);
-        //printf("LSM6DSL [acc/mg]:        %6ld\r\n", ACCGYRO_get_gyro_x());
-        //printf("LSM6DSL [gyro/mdps]:     %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
     }
 }
\ No newline at end of file

diff -r 1345f959c490 -r 45bd9ba6c0da servo.cpp
--- a/servo.cpp	Wed May 02 07:49:13 2018 +0000
+++ b/servo.cpp	Thu May 24 08:29:52 2018 +0000
@@ -1,88 +1,59 @@
 #include "mbed.h"
-#include "rtos.h"
-DigitalOut led3(LED3);
-RawSerial motor(PC_4,PC_5);
-unsigned char tx_buffer[3];
-unsigned char tx_buffer_ptr = 0;
-unsigned char tx_buffer_full = false;
-unsigned char rx_buffer[6];
-unsigned char rx_buffer_ptr = 0;
-unsigned char rx_buffer_full = false;
-signed short int motorcp[] = {8500,8500};
-unsigned short int Position;
-int MotorID;
-void Tx_interrupt();
-void Rx_interrupt();
-//EventQueue queue;
-//Thread t;
+#include "kondo1.h"
+#include "kondo2.h"
+#include "kondo3.h"
+#include "servo.h"
+extern EventQueue KONDO1_queue;
+extern EventQueue KONDO2_queue;
+extern EventQueue KONDO3_queue;
+double KODNO1_degrees1[] = {0,10,5,28,10,23,2};
+double KODNO1_degrees2[] = {10,10,10,10,10,10,10};
+double KODNO2_degrees1[] = {0,0,0,0,0,0,5,0,4,15};
+double KODNO3_degrees1[] = {15,15,15,15,15,15,15,15,15,15,15,15,-22,-19,20,-10,0,15};
+/*double KODNO3_degrees[] = {15,15,15,15,15};
+double KONDO3_Degrees_Initial[] = {0,0,0,0,0,0};
+double KONDO3_Degrees_Target[KONDO3_MAX_TOTAL];
+double KONDO3_Newdegree = 5;*/
 
-void motor_update(unsigned char Id, unsigned short int Position)
+void KONDO1_Degrees_Set()
 {
-    unsigned char id,lo,hi;
-    id=0x80|Id;
-    hi=(Position>>7)&0x007F;
-    lo=Position&0x007F;
-    NVIC_DisableIRQ(USART3_IRQn);
-    tx_buffer[0] = id;
-    tx_buffer[1] = hi;
-    tx_buffer[2] = lo;
-    NVIC_EnableIRQ(USART3_IRQn);
+        KONDO1_queue.call(KONDO1_Positions_Set,KODNO1_degrees1);         
 }
-void SERVO_task() 
+void KONDO2_Degrees_Set()
 {
-    led3 = !led3;
-    for(MotorID=0;MotorID<=1;MotorID++)
-    {
-        motor_update(MotorID,motorcp[MotorID]);
-        wait_us(900);
-    }
+        KONDO2_queue.call(KONDO2_Positions_Set,KODNO2_degrees1);
 }
-void SERVO_init() 
+void KONDO3_Degrees_Set()
 {
-    motor.format(8,Serial::Even,1);
-    motor.baud(115200);
-    rx_buffer_ptr = 0;
-    rx_buffer_full = false;
-    motor.attach(&Rx_interrupt, Serial::RxIrq);
-    motor.attach(&Tx_interrupt, Serial::TxIrq);
-    NVIC_EnableIRQ(USART3_IRQn);
-    //t.start(callback(&queue, &EventQueue::dispatch_forever));
-    //queue.call_every(1000, SERVO_init);
+        KONDO3_queue.call(KONDO3_Positions_Set,KODNO3_degrees1);
+}
 /*
+void KONDO2_Degrees_Set()
+{
     while(1)
     {
-        motorcp[0]=7500;
-        motorcp[1]=7500;
-        Thread::wait(1000);
-        motorcp[0]=8500;
-        motorcp[1]=8500;
-        Thread::wait(1000);
-    }
-*/
-}
-
-void Tx_interrupt() 
-{
-    while(motor.writeable())
-    {    
-        motor.putc(tx_buffer[tx_buffer_ptr]);
-        tx_buffer_ptr++;
-        if(tx_buffer_ptr==3)
-              tx_buffer_ptr = 0;
-    }
-    return;
-}
-void Rx_interrupt()
-{
-    while(motor.readable())
-    {
-        rx_buffer[rx_buffer_ptr] = motor.getc();
-        rx_buffer_ptr++;
-        if(rx_buffer_ptr==6)
+        unsigned char i;
+        unsigned char id;
+        for(i=0;i<=sizeof(KONDO2_DegreeOrder);i++)
         {
-            rx_buffer_full = true;
-            rx_buffer_ptr=0;
+            id = KONDO2_DegreeOrder[i];
+            KONDO2_Degrees_Target[id] = KONDO2_Degrees_Initial[id] + KONDO2_Newdegree[id]; 
+//            KONDO2_queue.call(KONDO2_Positions_Set,KONDO2_Degrees_Target);
         }
     }
-} 
- 
\ No newline at end of file
+}*/
+/*void KONDO3_Degrees_Set()
+{
+ while(1)
+    {
+        unsigned char id;
+        for(id=0;id<=5;id++)
+        {
+            KONDO3_Degrees_Target[id] = KONDO3_Degrees_Initial[id] + KONDO3_Newdegree;
+            KONDO3_queue.call(KONDO3_Positions_Set,KONDO3_Degrees_Target);
+        }
+        KONDO3_Newdegree = KONDO3_Newdegree + 10;
+        id=0;
+        wait(2);
+    }
+}*/
\ No newline at end of file

diff -r 1345f959c490 -r 45bd9ba6c0da servo.h
--- a/servo.h	Wed May 02 07:49:13 2018 +0000
+++ b/servo.h	Thu May 24 08:29:52 2018 +0000
@@ -1,7 +1,12 @@
 #ifndef __SERVO_H__
 #define __SERVO_H__
 
-void SERVO_init();
-void SERVO_task();
+#define KONDO_ORDER_SEND 32
+#define KONDO1_MAX_TOTAL 32
+#define KONDO2_MAX_TOTAL 32
+#define KONDO3_MAX_TOTAL 32
 
+void KONDO1_Degrees_Set();
+void KONDO2_Degrees_Set();
+void KONDO3_Degrees_Set();
 #endif
\ No newline at end of file