Revision 4:99891561a38b, committed 2018-05-25

Comitter:

mr_wang

Date:

Fri May 25 09:00:15 2018 +0000

Parent:

3:1345f959c490

Commit message:

NYP_DAD

Changed in this revision

diff -r 1345f959c490 -r 99891561a38b LSM6DSL.lib
--- a/LSM6DSL.lib	Wed May 02 07:49:13 2018 +0000
+++ b/LSM6DSL.lib	Fri May 25 09:00:15 2018 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/ST/code/LSM6DSL/#20ccff7dd652
+https://os.mbed.com/users/mr_wang/code/LSM6DSL/#6bc17982bf8a

diff -r 1345f959c490 -r 99891561a38b accgyro.cpp
--- a/accgyro.cpp	Wed May 02 07:49:13 2018 +0000
+++ b/accgyro.cpp	Fri May 25 09:00:15 2018 +0000
@@ -1,45 +1,109 @@
-#include "LSM6DSLSensor.h"
+#include "LSM6DSLSensor.h" 
 #include "accgyro.h"
 #include "mbed.h"
-#include "rtos.h"
+#include "behaviour.h"
 
-//Serial pc(USBTX, USBRX);
-DigitalOut led1(LED1);
+int i;
 int32_t ACCGYRO_gyro_axes[3];
 int32_t ACCGYRO_acc_axes[3];
+int32_t gyro_x[5];
+int32_t GYRO_Xf = 0;
+int32_t GYRO_Yf = 0;
+int32_t GYRO_Zf = 0;
+int32_t GYRO_Xfold;
+int32_t GYRO_Yfold;
+int32_t GYRO_Zfold;
+float GYRO_X_D;
+float GYRO_Y_D;
+float GYRO_Z_D;
+float GYRO_X_Dold;
+float GYRO_Y_Dold;
+float GYRO_Z_Dold;
+float GYRO_X_Time;
+float GYRO_Y_Time;
+float GYRO_Z_Time;
+
 static DevI2C devI2c(PB_11,PB_10);
 static LSM6DSLSensor acc_gyro(&devI2c,LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW,PD_11);
+
+Timer t;
+Ticker getdata;
 EventQueue ACCGYRO_queue;
+extern EventQueue BEHAVIOUR_queue;
+
 //Mutex ACCGYRO_mutex;
-
 int32_t ACCGYRO_get_gyro_x()
 {
     int32_t temp;
-    //ACCGYRO_mutex.lock();
     temp = ACCGYRO_gyro_axes[0];
-    //ACCGYRO_mutex.unlock();
+    return temp;
+}
+
+int32_t ACCGYRO_get_gyro_y()
+{
+    int32_t temp;
+    temp = ACCGYRO_gyro_axes[1];
     return temp;
 }
 
+int32_t ACCGYRO_get_gyro_z()
+{
+    int32_t temp;
+    temp = ACCGYRO_gyro_axes[2];
+    return temp;
+}
+/*
+void GYRO_get()
+{
+    
+    for (i=0; i<5; i++) 
+    {
+        gyro_x[i] = ACCGYRO_gyro_axes[0];
+    }
+    printf("%61d\n", &i);
+}
+*/
+
 void ACCGYRO_task()
 {
-    led1 = !led1;
-    //ACCGYRO_mutex.lock();
-    acc_gyro.get_x_axes(ACCGYRO_gyro_axes);
-    acc_gyro.get_g_axes(ACCGYRO_acc_axes);
-    //ACCGYRO_mutex.unlock();
-    //pc.printf("LSM6DSL [acc/mg]:        %6ld\r\n", ACCGYRO_gyro_axes[0]);
+    //int i;
+    acc_gyro.get_x_axes(ACCGYRO_acc_axes);
+    acc_gyro.get_g_axes(ACCGYRO_gyro_axes);
+    BEHAVIOUR_queue.call(BEHAVIOUR_gyro_x_set,ACCGYRO_gyro_axes[0]);
+    BEHAVIOUR_queue.call(BEHAVIOUR_gyro_y_set,ACCGYRO_gyro_axes[1]);
+    BEHAVIOUR_queue.call(BEHAVIOUR_gyro_z_set,ACCGYRO_gyro_axes[2]);
+    BEHAVIOUR_queue.call(BEHAVIOUR_acc_x_set,ACCGYRO_acc_axes[0]);
+    BEHAVIOUR_queue.call(BEHAVIOUR_acc_y_set,ACCGYRO_acc_axes[1]);
+    BEHAVIOUR_queue.call(BEHAVIOUR_acc_z_set,ACCGYRO_acc_axes[2]);
+    t. start();
+    for (i=0; i<5; i++)
+    {
+        gyro_x[i] = ACCGYRO_gyro_axes[0];
+    }
+    t.stop();
+    GYRO_X_Time = t.read();
+    //printf("The time taken was %f seconds\n", GYRO_X_Time);
+    t.reset();
+    GYRO_Xf = gyro_x[0];
+    GYRO_Xfold = gyro_x[4];
+    GYRO_X_D = (GYRO_Xf  + GYRO_Xfold)*0.5 * GYRO_X_Time *1000+ GYRO_X_Dold;
+    printf("%f\r\n", GYRO_X_D);
+/*
+    if(GYRO_X_D>180.0)
+        GYRO_X_D = GYRO_X_D - 360.0;
+    else
+    if(GYRO_X_D<-180.0)
+        GYRO_X_D = GYRO_X_D + 360.0;
+*/
 };
 
 void ACCGYRO_init()
 {
-    //pc.printf("Initing LSM6DSL\r\n", ACCGYRO_gyro_axes[0]);
+    printf("Initing LSM6DSL\r\n");
     acc_gyro.init(NULL);
     acc_gyro.enable_x();
     acc_gyro.enable_g();
-    //RtosTimer ACCGYRO_Timer(&ACCGYRO_task,osTimerPeriodic);
-    //ACCGYRO_Timer.start(100);
-    //pc.printf("Init done\r\n");
-    //ACCGYRO_queue.call_every(1000, ACCGYRO_task);
-    //ACCGYRO_queue.dispatch();
+    printf("Init done\r\n");
+    ACCGYRO_queue.call_every(100, ACCGYRO_task);
+    ACCGYRO_queue.dispatch();
 };

diff -r 1345f959c490 -r 99891561a38b behaviour.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/behaviour.cpp	Fri May 25 09:00:15 2018 +0000
@@ -0,0 +1,89 @@
+#include "mbed.h"
+#include "LSM6DSLSensor.h"
+#include "accgyro.h"
+
+DigitalOut led4(LED4);
+int32_t BEHAVIOUR_gyro_x;
+int32_t BEHAVIOUR_gyro_y;
+int32_t BEHAVIOUR_gyro_z;
+int32_t BEHAVIOUR_acc_x;
+int32_t BEHAVIOUR_acc_y;
+int32_t BEHAVIOUR_acc_z;
+
+EventQueue BEHAVIOUR_queue;
+
+void BEHAVIOUR_acc_x_set(int32_t temp)
+{
+    BEHAVIOUR_acc_x = temp;
+}
+
+void BEHAVIOUR_acc_y_set(int32_t temp)
+{
+    BEHAVIOUR_acc_y = temp;
+}
+
+void BEHAVIOUR_acc_z_set(int32_t temp)
+{
+    BEHAVIOUR_acc_z = temp;
+}
+
+int32_t BEHAVIOUR_acc_x_get()
+{
+    return BEHAVIOUR_acc_x;
+}
+
+int32_t BEHAVIOUR_acc_y_get()
+{
+    return BEHAVIOUR_acc_y;
+}
+
+int32_t BEHAVIOUR_acc_z_get()
+{
+    return BEHAVIOUR_acc_z;
+}
+
+void BEHAVIOUR_gyro_x_set(int32_t temp)
+{
+    BEHAVIOUR_gyro_x = temp;
+}
+
+void BEHAVIOUR_gyro_y_set(int32_t temp)
+{
+    BEHAVIOUR_gyro_y = temp;
+}
+
+void BEHAVIOUR_gyro_z_set(int32_t temp)
+{
+    BEHAVIOUR_gyro_z = temp;
+}
+
+int32_t BEHAVIOUR_gyro_x_get()
+{
+    return BEHAVIOUR_gyro_x;
+}
+
+int32_t BEHAVIOUR_gyro_y_get()
+{
+    return BEHAVIOUR_gyro_y;
+}
+
+int32_t BEHAVIOUR_gyro_z_get()
+{
+    return BEHAVIOUR_gyro_z;
+}
+
+void BEHAVIOUR_task()
+{
+    //printf("GYRO_x:        %6ld\r\n", BEHAVIOUR_gyro_x/1000);
+    //printf("GYRO_y:        %6ld\r\n", BEHAVIOUR_gyro_y/2000);
+    //printf("GYRO_z:        %6ld\r\n", BEHAVIOUR_gyro_z/2000);
+    //printf("ACC_x:        %6ld\r\n", BEHAVIOUR_acc_x);
+    //printf("ACC_y:        %6ld\r\n", BEHAVIOUR_acc_y/(160*6));
+    //printf("ACC_z:        %6ld\r\n", BEHAVIOUR_acc_z/(160*6));
+}
+
+void BEHAVIOUR_init()
+{
+    BEHAVIOUR_queue.call_every(100, BEHAVIOUR_task);
+    BEHAVIOUR_queue.dispatch();    
+}

diff -r 1345f959c490 -r 99891561a38b behaviour.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/behaviour.h	Fri May 25 09:00:15 2018 +0000
@@ -0,0 +1,19 @@
+#ifndef __BEHAVIOUR_H__
+#define __BEHAVIOUR_H__
+
+void BEHAVIOUR_acc_x_set(int32_t temp);
+void BEHAVIOUR_acc_y_set(int32_t temp);
+void BEHAVIOUR_acc_z_set(int32_t temp);
+void BEHAVIOUR_gyro_x_set(int32_t temp);
+void BEHAVIOUR_gyro_y_set(int32_t temp);
+void BEHAVIOUR_gyro_z_set(int32_t temp);
+int32_t BEHAVIOUR_acc__get();
+int32_t BEHAVIOUR_acc_y_get();
+int32_t BEHAVIOUR_acc_z_get();
+int32_t BEHAVIOUR_gyro_x_get();
+int32_t BEHAVIOUR_gyro_y_get();
+int32_t BEHAVIOUR_gyro_z_get();
+void BEHAVIOUR_init();
+void BEHAVIOUR_task();
+
+#endif
\ No newline at end of file

diff -r 1345f959c490 -r 99891561a38b control.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/control.cpp	Fri May 25 09:00:15 2018 +0000
@@ -0,0 +1,162 @@
+#include "mbed.h"
+#include "control.h"
+#include "behaviour.h"
+
+void CONTROL_OnOff(CONTROLLER* control)
+{
+    if(control->presentvalue > control->setpoint)
+        control->output = control->upperlimit;//upperlimit is negative value
+    else
+    if(control->presentvalue < control->setpoint)
+        control->output = control->lowerlimit;
+    else
+        control->output = 0.0;
+}
+
+void CONTROL_P(CONTROLLER* control)
+{       
+    control->error = control->setpoint - control->presentvalue;
+    control->error_p = control->error;  
+    control->output = control->gain_p * control->error_p;
+    
+    if(control->output < control->lowerdeadband || control->output > control->upperdeadband)
+    {   
+        if(control->output < control->lowerlimit) 
+            control->output = control->lowerlimit;  
+        if(control->output > control->upperlimit) 
+            control->output = control->upperlimit;
+    }
+    else
+        control->output = 0.0;
+}
+
+void CONTROL_PI(CONTROLLER* control)
+{
+    double error_old;
+    
+    error_old = control->error;
+    control->error = control->setpoint - control->presentvalue;
+    
+    control->error_p = control->error;
+    control->error_i += error_old;
+    control->output = (control->gain_p * control->error_p)+(control->gain_i * control->error_i);
+
+    if(control->output < control->lowerdeadband || control->output > control->upperdeadband)
+    {   
+        if(control->output < control->lowerlimit) 
+            control->output = control->lowerlimit;  
+        if(control->output > control->upperlimit) 
+            control->output = control->upperlimit;
+    }
+    else
+        control->output = 0.0;
+}
+
+void CONTROL_PD(CONTROLLER* control)
+{
+    double error_old;
+    
+    error_old = control->error;
+    control->error = control->setpoint - control->presentvalue;
+    control->error_p = control->error;
+    control->error_d = control->error - error_old;
+    control->output = (control->gain_p * control->error_p)+(control->gain_d * control->error_d);
+
+    if(control->output < control->lowerdeadband || control->output > control->upperdeadband)
+    {   
+        if(control->output < control->lowerlimit) 
+            control->output = control->lowerlimit;  
+        if(control->output > control->upperlimit) 
+            control->output = control->upperlimit;
+    }
+    else
+        control->output = 0.0;
+}
+
+void CONTROL_PID(CONTROLLER* control)
+{
+    double error_old;
+    
+    error_old = control->error;
+    control->error = control->setpoint - control->presentvalue;
+    
+    control->error_p = control->error;
+    control->error_i += error_old;
+    control->error_d = control->error - error_old;
+    control->output = (control->gain_p * control->error_p)+(control->gain_i * control->error_i)+(control->gain_d * control->error_d);
+
+    if(control->output < control->lowerdeadband || control->output > control->upperdeadband)
+    {   
+        if(control->output < control->lowerlimit) 
+            control->output = control->lowerlimit;  
+        if(control->output > control->upperlimit) 
+            control->output = control->upperlimit;
+    }
+    else
+        control->output = 0.0;
+}
+
+void CONTROL_InputMode_Set(CONTROLLER* control, unsigned char input, double mode)
+{
+    control->input = input;
+    control->mode = mode;
+}
+void CONTROL_Setpoint_Set(CONTROLLER* control, double sp)
+{
+    control->setpoint = sp;
+}
+void CONTROL_Gain_Set(CONTROLLER* control, double p, double i, double d)
+{
+    control->gain_p = p;
+    control->gain_i = i;
+    control->gain_d = d;
+}
+void CONTROL_Limit_Set(CONTROLLER* control, double upper, double lower)
+{
+    control->upperlimit = upper;
+    control->lowerlimit = lower;
+}
+void CONTROL_Deadband_Set(CONTROLLER* control, double upper, double lower)
+{
+    control->upperdeadband = upper;
+    control->lowerdeadband = lower;
+}
+
+void CONTROL_PresentValue_Set(CONTROLLER* control, double pv)
+{
+    control->presentvalue = pv;
+}
+    
+double CONTROL_Output_Get(CONTROLLER* control)
+{
+    switch(control->mode)
+    {
+        case CONTROL_MODE_NONE:
+            break;
+        case CONTROL_MODE_ONOFF:
+            CONTROL_OnOff(control);
+            break;
+        case CONTROL_MODE_P:
+            CONTROL_P(control);
+            break;
+        case CONTROL_MODE_PI:
+            CONTROL_PI(control);
+            break;
+        case CONTROL_MODE_PD:
+            CONTROL_PD(control);
+            break;
+        case CONTROL_MODE_PID:
+            CONTROL_PID(control);
+            break;      
+    }
+    return control->output; 
+}    
+
+void CONTROL_Config(void)
+{
+}
+
+void CONTROL_Task(void)                                         
+{
+
+}
\ No newline at end of file

diff -r 1345f959c490 -r 99891561a38b control.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/control.h	Fri May 25 09:00:15 2018 +0000
@@ -0,0 +1,54 @@
+#ifndef __CONTROL_H
+#define __CONTROL_H
+
+#define CONTROL_MODE_NONE   0
+#define CONTROL_MODE_ONOFF  1
+#define CONTROL_MODE_P      2
+#define CONTROL_MODE_PI     3
+#define CONTROL_MODE_PD     4
+#define CONTROL_MODE_PID    5
+
+typedef struct{
+    unsigned char input;
+
+    double presentvalue;
+
+    double setpoint;
+
+    double upperlimit;
+    double lowerlimit;
+
+    double upperdeadband;
+    double lowerdeadband;
+    
+    unsigned char mode;
+    double gain_p;
+    double gain_i;
+    double gain_d;
+        
+    double error;
+    double error_p;
+    double error_i;
+    double error_d;
+    
+    double output;
+}CONTROLLER;    
+
+void CONTROL_InputMode_Set(CONTROLLER* control, unsigned char input, double mode);
+void CONTROL_Setpoint_Set(CONTROLLER* control, double sp);
+void CONTROL_Gain_Set(CONTROLLER* control, double p, double i, double d);
+void CONTROL_Limit_Set(CONTROLLER* control, double upper, double lower);
+void CONTROL_Deadband_Set(CONTROLLER* control, double upper, double lower);
+void CONTROL_PresentValue_Set(CONTROLLER* control, double pv);
+double CONTROL_Output_Get(CONTROLLER* control);
+
+void CONTROL_OnOff(CONTROLLER* control);
+void CONTROL_P(CONTROLLER* control);
+void CONTROL_PID(CONTROLLER* control);
+void CONTROL_PI(CONTROLLER* control);
+void CONTROL_PD(CONTROLLER* control);
+ 
+void CONTROL_Config(void);
+void CONTROL_Task(void);
+
+#endif

diff -r 1345f959c490 -r 99891561a38b kondo1.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kondo1.cpp	Fri May 25 09:00:15 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 99891561a38b kondo1.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kondo1.h	Fri May 25 09:00:15 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 99891561a38b kondo2.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kondo2.cpp	Fri May 25 09:00:15 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 99891561a38b kondo2.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kondo2.h	Fri May 25 09:00:15 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 99891561a38b kondo3.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kondo3.cpp	Fri May 25 09:00:15 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 99891561a38b kondo3.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kondo3.h	Fri May 25 09:00:15 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 99891561a38b main.cpp
--- a/main.cpp	Wed May 02 07:49:13 2018 +0000
+++ b/main.cpp	Fri May 25 09:00:15 2018 +0000
@@ -1,33 +1,44 @@
 #include "mbed.h"
-#include "rtos.h"
 #include "servo.h"
 #include "accgyro.h"
-//Serial pc(USBTX, USBRX);
-DigitalOut led2(LED2);
-Thread event_thread;
+#include "behaviour.h"
+#include "kondo2.h"
+
+//Thread event_thread;
 Thread ACCGYRO_thread;
 Thread SERVO_thread;
+Thread BEHAVIOUR_thread;
 EventQueue event_queue;
+Thread KONDO1_thread;
+Thread KONDO2_thread;
+Thread KONDO3_thread;
+Thread KONDO1_Degrees_Set_thread;
+Thread KONDO2_Degrees_Set_thread;
 
 int main()
 {   
+    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);
     ACCGYRO_thread.start(ACCGYRO_init);
-    SERVO_thread.start(SERVO_init);
+    //SERVO_thread.start(SERVO_init);
+    BEHAVIOUR_thread.start(BEHAVIOUR_init);
     //SERVO_init();
     //event_thread.start(callback(&event_queue, &EventQueue::dispatch_forever));
     //ACCGYRO_init();
-    event_thread.start(callback(&event_queue, &EventQueue::dispatch_forever));
+    //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);
+    //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();
     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("*");
@@ -35,4 +46,4 @@
         //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 99891561a38b servo.cpp
--- a/servo.cpp	Wed May 02 07:49:13 2018 +0000
+++ b/servo.cpp	Fri May 25 09:00:15 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 99891561a38b servo.h
--- a/servo.h	Wed May 02 07:49:13 2018 +0000
+++ b/servo.h	Fri May 25 09:00:15 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