Lin ShengKun / Mbed OS NYP_FYP

FYP

Dependencies:   LSM6DSL

Fork of humanoid by Junjie Wang

servo.cpp@4:45bd9ba6c0da, 2018-05-24 (annotated)

Committer:
ha731548874
Date:
Thu May 24 08:29:52 2018 +0000
Revision:
4:45bd9ba6c0da
Parent:
3:1345f959c490 
NYP_humaniod_FYP

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ha731548874 1:cde16b5e604d 1 #include "mbed.h"
ha731548874 4:45bd9ba6c0da 2 #include "kondo1.h"
ha731548874 4:45bd9ba6c0da 3 #include "kondo2.h"
ha731548874 4:45bd9ba6c0da 4 #include "kondo3.h"
ha731548874 4:45bd9ba6c0da 5 #include "servo.h"
ha731548874 4:45bd9ba6c0da 6 extern EventQueue KONDO1_queue;
ha731548874 4:45bd9ba6c0da 7 extern EventQueue KONDO2_queue;
ha731548874 4:45bd9ba6c0da 8 extern EventQueue KONDO3_queue;
ha731548874 4:45bd9ba6c0da 9 double KODNO1_degrees1[] = {0,10,5,28,10,23,2};
ha731548874 4:45bd9ba6c0da 10 double KODNO1_degrees2[] = {10,10,10,10,10,10,10};
ha731548874 4:45bd9ba6c0da 11 double KODNO2_degrees1[] = {0,0,0,0,0,0,5,0,4,15};
ha731548874 4:45bd9ba6c0da 12 double KODNO3_degrees1[] = {15,15,15,15,15,15,15,15,15,15,15,15,-22,-19,20,-10,0,15};
ha731548874 4:45bd9ba6c0da 13 /*double KODNO3_degrees[] = {15,15,15,15,15};
ha731548874 4:45bd9ba6c0da 14 double KONDO3_Degrees_Initial[] = {0,0,0,0,0,0};
ha731548874 4:45bd9ba6c0da 15 double KONDO3_Degrees_Target[KONDO3_MAX_TOTAL];
ha731548874 4:45bd9ba6c0da 16 double KONDO3_Newdegree = 5;*/
mr_wang 3:1345f959c490 17
ha731548874 4:45bd9ba6c0da 18 void KONDO1_Degrees_Set()
ha731548874 1:cde16b5e604d 19 {
ha731548874 4:45bd9ba6c0da 20 KONDO1_queue.call(KONDO1_Positions_Set,KODNO1_degrees1);
ha731548874 1:cde16b5e604d 21 }
ha731548874 4:45bd9ba6c0da 22 void KONDO2_Degrees_Set()
ha731548874 1:cde16b5e604d 23 {
ha731548874 4:45bd9ba6c0da 24 KONDO2_queue.call(KONDO2_Positions_Set,KODNO2_degrees1);
ha731548874 1:cde16b5e604d 25 }
ha731548874 4:45bd9ba6c0da 26 void KONDO3_Degrees_Set()
ha731548874 1:cde16b5e604d 27 {
ha731548874 4:45bd9ba6c0da 28 KONDO3_queue.call(KONDO3_Positions_Set,KODNO3_degrees1);
ha731548874 4:45bd9ba6c0da 29 }
mr_wang 3:1345f959c490 30 /*
ha731548874 4:45bd9ba6c0da 31 void KONDO2_Degrees_Set()
ha731548874 4:45bd9ba6c0da 32 {
ha731548874 1:cde16b5e604d 33 while(1)
ha731548874 1:cde16b5e604d 34 {
ha731548874 4:45bd9ba6c0da 35 unsigned char i;
ha731548874 4:45bd9ba6c0da 36 unsigned char id;
ha731548874 4:45bd9ba6c0da 37 for(i=0;i<=sizeof(KONDO2_DegreeOrder);i++)
ha731548874 1:cde16b5e604d 38 {
ha731548874 4:45bd9ba6c0da 39 id = KONDO2_DegreeOrder[i];
ha731548874 4:45bd9ba6c0da 40 KONDO2_Degrees_Target[id] = KONDO2_Degrees_Initial[id] + KONDO2_Newdegree[id];
ha731548874 4:45bd9ba6c0da 41 // KONDO2_queue.call(KONDO2_Positions_Set,KONDO2_Degrees_Target);
ha731548874 1:cde16b5e604d 42 }
ha731548874 1:cde16b5e604d 43 }
ha731548874 4:45bd9ba6c0da 44 }*/
ha731548874 4:45bd9ba6c0da 45 /*void KONDO3_Degrees_Set()
ha731548874 4:45bd9ba6c0da 46 {
ha731548874 4:45bd9ba6c0da 47 while(1)
ha731548874 4:45bd9ba6c0da 48 {
ha731548874 4:45bd9ba6c0da 49 unsigned char id;
ha731548874 4:45bd9ba6c0da 50 for(id=0;id<=5;id++)
ha731548874 4:45bd9ba6c0da 51 {
ha731548874 4:45bd9ba6c0da 52 KONDO3_Degrees_Target[id] = KONDO3_Degrees_Initial[id] + KONDO3_Newdegree;
ha731548874 4:45bd9ba6c0da 53 KONDO3_queue.call(KONDO3_Positions_Set,KONDO3_Degrees_Target);
ha731548874 4:45bd9ba6c0da 54 }
ha731548874 4:45bd9ba6c0da 55 KONDO3_Newdegree = KONDO3_Newdegree + 10;
ha731548874 4:45bd9ba6c0da 56 id=0;
ha731548874 4:45bd9ba6c0da 57 wait(2);
ha731548874 4:45bd9ba6c0da 58 }
ha731548874 4:45bd9ba6c0da 59 }*/