servo.cpp

00001 #include "mbed.h"
00002 #include "kondo1.h"
00003 #include "kondo2.h"
00004 #include "kondo3.h"
00005 #include "servo.h"
00006 extern EventQueue KONDO1_queue;
00007 extern EventQueue KONDO2_queue;
00008 extern EventQueue KONDO3_queue;
00009 double KODNO1_degrees1[] = {0,10,5,28,10,23,2};
00010 double KODNO1_degrees2[] = {10,10,10,10,10,10,10};
00011 double KODNO2_degrees1[] = {0,0,0,0,0,0,5,0,4,15};
00012 double KODNO3_degrees1[] = {15,15,15,15,15,15,15,15,15,15,15,15,-22,-19,20,-10,0,15};
00013 /*double KODNO3_degrees[] = {15,15,15,15,15};
00014 double KONDO3_Degrees_Initial[] = {0,0,0,0,0,0};
00015 double KONDO3_Degrees_Target[KONDO3_MAX_TOTAL];
00016 double KONDO3_Newdegree = 5;*/
00017 
00018 void KONDO1_Degrees_Set()
00019 {
00020         KONDO1_queue.call(KONDO1_Positions_Set,KODNO1_degrees1);         
00021 }
00022 void KONDO2_Degrees_Set()
00023 {
00024         KONDO2_queue.call(KONDO2_Positions_Set,KODNO2_degrees1);
00025 }
00026 void KONDO3_Degrees_Set()
00027 {
00028         KONDO3_queue.call(KONDO3_Positions_Set,KODNO3_degrees1);
00029 }
00030 /*
00031 void KONDO2_Degrees_Set()
00032 {
00033     while(1)
00034     {
00035         unsigned char i;
00036         unsigned char id;
00037         for(i=0;i<=sizeof(KONDO2_DegreeOrder);i++)
00038         {
00039             id = KONDO2_DegreeOrder[i];
00040             KONDO2_Degrees_Target[id] = KONDO2_Degrees_Initial[id] + KONDO2_Newdegree[id]; 
00041 //            KONDO2_queue.call(KONDO2_Positions_Set,KONDO2_Degrees_Target);
00042         }
00043     }
00044 }*/
00045 /*void KONDO3_Degrees_Set()
00046 {
00047  while(1)
00048     {
00049         unsigned char id;
00050         for(id=0;id<=5;id++)
00051         {
00052             KONDO3_Degrees_Target[id] = KONDO3_Degrees_Initial[id] + KONDO3_Newdegree;
00053             KONDO3_queue.call(KONDO3_Positions_Set,KONDO3_Degrees_Target);
00054         }
00055         KONDO3_Newdegree = KONDO3_Newdegree + 10;
00056         id=0;
00057         wait(2);
00058     }
00059 }*/