Diff: servo.cpp

Revision:

4:45bd9ba6c0da

Parent:

3:1345f959c490

--- 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