Junjie Wang / Mbed OS a_NYP_humanoid_copy

NYP_Humanoid_robot_FYP_2018

Dependencies:   LSM6DSL

Fork of b_NYP_humanoid by Junjie Wang

servo.cpp@4:99891561a38b, 2018-05-25 (annotated)

Committer:
mr_wang
Date:
Fri May 25 09:00:15 2018 +0000
Revision:
4:99891561a38b
Parent:
3:1345f959c490 
NYP_DAD

Who changed what in which revision?

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