Diff: Controlor.cpp

Revision:

23:0927e605af4b

Parent:

22:bf5aa20b9df0

--- a/Controlor.cpp	Fri Sep 06 08:36:21 2013 +0000
+++ b/Controlor.cpp	Fri Nov 22 00:30:42 2013 +0000
@@ -4,13 +4,11 @@
 #include "Controlor.h"
 #include "HomePosition.h"
 
-Ticker tick;
-
-const float TIMESTEP = 0.005;
+const float TIMESTEP = 0.01;
 
 //detach may be better to controled from Controlor class
 
-Controlor::Controlor(uint16_t* data) : playing(false), attached(false)
+Controlor::Controlor(uint16_t* data) : playing(false), attached(false), servo_size(10)
 {
     //pwm = new PWM();
     pwm = new Adafruit_PWMServoDriver(p9, p10);
@@ -18,20 +16,19 @@
     pwm->setPrescale(64);    //This value is decided for 10ms interval.
     pwm->setI2Cfreq(400000); //400kHz
     
-    for (int i = 0; i < 16; ++i) {
-        pwm->setDuty(i, HOMEPOS[i]);
-    }
+    rs300 = new RS300(p28, p27);
+    rs300->on_all_servo();
     
-    //comu = new SCI(p28, p27);
-    comu = new Console(USBTX, USBRX);
+    home();
     
-    LocalFileSystem* local = new LocalFileSystem("local");
+    //comu = new Console(p13, p14);
+    comu = new Console(USBTX, USBRX);
+
+    mode = 0;
     
-    read("/local/motion.csv", data);
-    set(data);
-    
-    // Motion 0 is Home position
-    //setmotion(0);
+    online = new OnlineMotion(0.7, TIMESTEP, servo_size, pwm, rs300, &playing);
+    cpg = new CPG(TIMESTEP, pwm, &playing);
+    playing = false;
 }
 
 Controlor::~Controlor()
@@ -45,12 +42,25 @@
     delete pwm;
 }
 
+void Controlor::home() {
+	int pwm_servo_num = 10, serial_servo_num = 10;
+
+	for (int i = 0; i < pwm_servo_num; ++i) {
+		pwm->setDuty(i, HOMEPOS[i]);
+	}
+	//std::vector<uint16_t> buf(&HOMEPOS[pwm_servo_num], &HOMEPOS[pwm_servo_num + serial_servo_num]);
+	std::vector<uint16_t> buf;
+	for (int i = pwm_servo_num; i < pwm_servo_num + serial_servo_num; ++i) {
+		buf.push_back(HOMEPOS[i]);
+	}
+	rs300->send_servo_pos(1, buf);
+}
+
 void Controlor::read(const string& filename, uint16_t* data)
 {
     CSV csv;
     pose_size = new int;//<-This code is suspicious
     csv.read(filename, data, &servo_size, &motion_size, pose_size);
-    //readMotion(filename, data, servo_size, motion_size, pose_size); // not so good at speed and has bug in handling float motion value.
 }
 
 void Controlor::set(uint16_t* data)
@@ -86,38 +96,47 @@
     //if (!motion.playing) {
         //motion = new OfflineMotion(motions[id], pose_size[id], servo_size, pwm, &playing);
         //tick.attach(motion, &Motion::step, TIMESTEP);
-        attached = true;
+        //attached = true;
         //offline = new OfflineMotion(motions[id], pose_size[id], servo_size, pwm, &playing);
         //tick.attach(offline, &OfflineMotion::step, TIMESTEP);
-        online = new OnlineMotion(0.5, TIMESTEP, servo_size, pwm, &playing);
-        tick.attach(online, &OnlineMotion::step, TIMESTEP);
+        //online = new OnlineMotion(0.5, TIMESTEP, servo_size, pwm, &playing);
+        //tick.attach(online, &OnlineMotion::step, TIMESTEP);
+        //cpg = new CPG(TIMESTEP, pwm, &playing);
+        //tick.attach(cpg, &CPG::step, TIMESTEP);
     //}
 }
 
 void Controlor::control()
 {
-    /*if (!playing) {
-        if (attached) {
-            tick.detach();
-            delete online;
-            attached = false;
-        } else {
-            setmotion(1);
-        }
-    }*/
-    
-    //setmotion(1);
+    if (playing) {
+        //online->step();
+        cpg->step();
+    }
     char head = comu->getheader();
     if (head == 'a') {
         int id = comu->getid();
         if (checkid(id)) {
-            //setmotion(id);
+            setmotion(id);
         }
     } else if (head == 'b') {
         int id = comu->getid();
         uint16_t val = comu->get_int_cr();
-        //wait(1);
         pwm->setDuty(id, val);
-    }
-    
-}
\ No newline at end of file
+    } else if (head == 'c') {
+        cpg->change_param(0.1, 0.1, 0.1);
+    } else if (head == 'd') {
+        playing = true;
+    } else if (head == 'e') {
+    	playing = false;
+    } else if (head == 'e') {
+        comu->printf("Omega:\r\n");
+        float omega = comu->get_float_cr();
+        comu->printf("K:\r\n");
+        float k = comu->get_float_cr();
+        cpg->change_param(omega, k, 0);
+    } else if (head == 'f') {
+    	home();
+    } else if (head == 'p') {
+    	comu->showOnline(online);
+    } 
+}