Revision 22:bf5aa20b9df0, committed 2013-09-06

Comitter:

syundo0730

Date:

Fri Sep 06 08:36:21 2013 +0000

Parent:

21:a54bcab078ed

Child:

23:0927e605af4b

Commit message:

Subtile change.; Home position was changed.; SCI class was renamed to Console class and function was modified.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Console.cpp	Fri Sep 06 08:36:21 2013 +0000
@@ -0,0 +1,92 @@
+#include <sstream>
+
+#include "Console.h"
+
+Console::Console(PinName tx, PinName rx)
+{
+    serial = new Serial(tx, rx);
+}
+
+Console::~Console()
+{
+    delete serial;
+}
+
+uint8_t Console::getheader()
+{
+    uint8_t comm = getc_nowait();
+    if (comm != 0) {
+        serial->printf("Mode:%c \r\n", comm);
+    }
+    return comm;
+}
+
+int Console::getid()
+{
+    int id;
+    stringstream sstr;
+    serial->printf("Enter the ID: \r\n");
+    sstr << getc_wait();
+    sstr >> id;
+    serial->printf("Servo ID:%d \r\n", id);
+    return id;
+}
+
+uint16_t Console::get_int_cr()
+{
+    stringstream sstr;
+    serial->printf("Enter the Servo value. And push Enter.: \r\n");
+    while (1) {
+        uint8_t tmp = getc_wait();
+        serial->printf("%c", tmp);
+        if (tmp == '\r') {
+            serial->printf("\r\n");
+            break;
+            }
+        sstr << tmp;
+    }
+    uint16_t val;
+    sstr >> val;
+    serial->printf("Servo value:%d \r\n", val);
+    return val;
+}
+
+uint8_t Console::getc_wait()
+{
+    while (!serial->readable());
+    uint8_t val = serial->getc();
+    return val;
+}
+
+uint8_t Console::getc_nowait()
+{
+    uint8_t buf = 0;
+    if (serial->readable()) {
+        buf = serial->getc();
+    }
+    return buf;
+}
+
+void Console::printf(char* str)
+{
+    serial->printf(str);
+}
+
+/*uint16_t Console::readint()
+{
+    uint8_t buff[2];
+    buff[0] = getc_wait();
+    buff[1] = getc_wait();
+    uint16_t val;
+    val = (uint16_t)(buff[1] << 8) | (uint16_t)buff[0];
+    return val;
+}*/
+
+/*void sendint(uint16_t val)
+{
+    uint8_t buff[16];
+    buff[0] = (uint8_t)(val & 0x00FF);
+    buff[1] = (uint8_t)(val >> 8);
+    pc.putc(buff[0]);
+    pc.putc(buff[1]);
+}*/
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Console.h	Fri Sep 06 08:36:21 2013 +0000
@@ -0,0 +1,26 @@
+#ifndef Console_H_2013_02_03_
+#define Console_H_2013_02_03_
+
+#include "mbed.h"
+
+class Console
+{
+  public:
+    Console(PinName tx, PinName rx);
+    ~Console();
+    
+  public:
+    int getid();
+    uint8_t getheader();
+    uint16_t get_int_cr();
+    void printf(char* str);
+    
+  private:
+    uint8_t getc_wait();
+    uint8_t getc_nowait();
+    
+  private:
+    Serial* serial;
+};
+
+#endif
\ No newline at end of file

--- a/Controlor.cpp	Wed Sep 04 03:59:40 2013 +0000
+++ b/Controlor.cpp	Fri Sep 06 08:36:21 2013 +0000
@@ -2,11 +2,11 @@
 #include <string>
 
 #include "Controlor.h"
+#include "HomePosition.h"
 
 Ticker tick;
 
-const float TIMESTEP = 0.01;
-
+const float TIMESTEP = 0.005;
 
 //detach may be better to controled from Controlor class
 
@@ -18,8 +18,12 @@
     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]);
+    }
+    
     //comu = new SCI(p28, p27);
-    comu = new SCI(USBTX, USBRX);
+    comu = new Console(USBTX, USBRX);
     
     LocalFileSystem* local = new LocalFileSystem("local");
     
@@ -27,7 +31,7 @@
     set(data);
     
     // Motion 0 is Home position
-    setmotion(0);
+    //setmotion(0);
 }
 
 Controlor::~Controlor()
@@ -83,36 +87,37 @@
         //motion = new OfflineMotion(motions[id], pose_size[id], servo_size, pwm, &playing);
         //tick.attach(motion, &Motion::step, TIMESTEP);
         attached = true;
-        offline = new OfflineMotion(motions[id], pose_size[id], servo_size, pwm, &playing);
-        tick.attach(offline, &OfflineMotion::step, TIMESTEP);
-        //online = new OnlineMotion(1.0, TIMESTEP, servo_size, pwm, &playing);
-        //tick.attach(online, &OnlineMotion::step, TIMESTEP);
+        //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);
     //}
 }
 
 void Controlor::control()
 {
-    if (!playing) {
+    /*if (!playing) {
         if (attached) {
             tick.detach();
-            delete offline;
+            delete online;
             attached = false;
         } else {
             setmotion(1);
         }
-    }
+    }*/
     
     //setmotion(1);
-    /*char head = comu->getheader();
-    if (head == 'A') {
+    char head = comu->getheader();
+    if (head == 'a') {
         int id = comu->getid();
         if (checkid(id)) {
-            setmotion(id);
+            //setmotion(id);
         }
-    } else if (head == 'B') {
+    } else if (head == 'b') {
         int id = comu->getid();
-        uint16_t val = comu->getservoval();
-        pwm->SetDuty(id, (uint32_t)val);
-    }*/
+        uint16_t val = comu->get_int_cr();
+        //wait(1);
+        pwm->setDuty(id, val);
+    }
     
 }
\ No newline at end of file

--- a/Controlor.h	Wed Sep 04 03:59:40 2013 +0000
+++ b/Controlor.h	Fri Sep 06 08:36:21 2013 +0000
@@ -8,7 +8,7 @@
 #include "OfflineMotion.h"
 #include "OnlineMotion.h"
 
-#include "SCI.h"
+#include "Console.h"
 //#include "PWM.h"
 #include "Adafruit_PWMServoDriver.h"
 
@@ -46,7 +46,7 @@
   private:
     bool playing;
     bool attached;
-    SCI* comu;
+    Console* comu;
     //PWM* pwm;
     Adafruit_PWMServoDriver* pwm;
 };

--- a/HomePosition.h	Wed Sep 04 03:59:40 2013 +0000
+++ b/HomePosition.h	Fri Sep 06 08:36:21 2013 +0000
@@ -1,26 +1,28 @@
 const uint16_t HOMEPOS[] = {
-1570,
-1490,
+1525,
+1510,
+1240,
+1520,
+1510,
+
+1515,
+1520,
+1830,
+1530,
 1500,
-1820,
-1515,
-1500,
+
 0,
 0,
-1500,
-1500,
-1500,
-1220,
-1500,
-1500,
+0,
+0,
+0,
+0,
+0,
 0,
 0,
-1520,
-1520,
-1520,
-1520,
-1520,
-1520,
-1520,
-1520
+0,
+0,
+0,
+0,
+0
 };
\ No newline at end of file

--- a/Motion/OnlineMotion.cpp	Wed Sep 04 03:59:40 2013 +0000
+++ b/Motion/OnlineMotion.cpp	Fri Sep 06 08:36:21 2013 +0000
@@ -98,16 +98,16 @@
     invertkinematics(rpos, rtheta);
     
     uint32_t pos[] = {
-        HOMEPOS[0] + ltheta[0],
-        HOMEPOS[1] - ltheta[1],
-        HOMEPOS[2] - ltheta[2],
-        HOMEPOS[3] + ltheta[3],
-        HOMEPOS[4] + ltheta[0],
-        HOMEPOS[5] + rtheta[0],
-        HOMEPOS[6] + rtheta[1],
-        HOMEPOS[7] + rtheta[2],
-        HOMEPOS[8] - rtheta[3],
-        HOMEPOS[9] + rtheta[0],
+        HOMEPOS[0] + rtheta[0],
+        HOMEPOS[1] + rtheta[1],
+        HOMEPOS[2] + rtheta[2],
+        HOMEPOS[3] - rtheta[3],
+        HOMEPOS[4] + rtheta[0],
+        HOMEPOS[5] + ltheta[0],
+        HOMEPOS[6] - ltheta[1],
+        HOMEPOS[7] - ltheta[2],
+        HOMEPOS[8] + ltheta[3],
+        HOMEPOS[9] + ltheta[0],
         HOMEPOS[10],
         HOMEPOS[11],
         HOMEPOS[12],
@@ -124,9 +124,9 @@
         HOMEPOS[23]
     };
     
+    __disable_irq();
     for (int i = 0; i < m_NUM_MAX; ++i) {
-        __disable_irq();
         pwm->setDuty(i, pos[i]);
-        __enable_irq();
     }
+    __enable_irq();
 }
\ No newline at end of file

--- a/SCI.cpp	Wed Sep 04 03:59:40 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,85 +0,0 @@
-
-#include "SCI.h"
-
-SCI::SCI(PinName tx, PinName rx)
-{
-    serial = new Serial(tx, rx);
-}
-
-SCI::~SCI()
-{
-    delete serial;
-}
-
-char SCI::getheader()
-{
-    char comm = getc_nowait();
-    if (comm != 0x00) {
-        serial->putc(comm);
-    }
-    return comm;
-}
-
-int SCI::getid()
-{
-    int id = 255;
-    stringstream sstr;
-    sstr << getc_wait();
-    sstr >> id;
-    serial->printf("id = %d \r\n", id);
-    return id;
-}
-
-uint16_t SCI::getservoval()
-{
-    uint16_t val = 1500;
-    stringstream sstr;
-    while (!serial->readable());
-    for (int i = 0; i < 4; ++i) {
-        sstr << getc_wait();
-    }
-    sstr >> val;
-    serial->printf("val:%d", val);
-    serial->printf("\r\n");
-    return val;
-}
-
-char SCI::getc_wait()
-{
-    while (!serial->readable());
-    char val = serial->getc();
-    return val;
-}
-
-char SCI::getc_nowait()
-{
-    uint8_t buf = 0;
-    if (serial->readable()) {
-        buf = serial->getc();
-    }
-    return buf;
-}
-
-void SCI::printf(char* str)
-{
-    serial->printf(str);
-}
-
-/*uint16_t SCI::readint()
-{
-    uint8_t buff[2];
-    buff[0] = getc_wait();
-    buff[1] = getc_wait();
-    uint16_t val;
-    val = (uint16_t)(buff[1] << 8) | (uint16_t)buff[0];
-    return val;
-}*/
-
-/*void sendint(uint16_t val)
-{
-    uint8_t buff[16];
-    buff[0] = (uint8_t)(val & 0x00FF);
-    buff[1] = (uint8_t)(val >> 8);
-    pc.putc(buff[0]);
-    pc.putc(buff[1]);
-}*/
\ No newline at end of file

--- a/SCI.h	Wed Sep 04 03:59:40 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,27 +0,0 @@
-#ifndef SCI_H_2013_02_03_
-#define SCI_H_2013_02_03_
-
-#include "mbed.h"
-#include <sstream>
-
-class SCI
-{
-  public:
-    SCI(PinName tx, PinName rx);
-    ~SCI();
-    
-  public:
-    int getid();
-    char getheader();
-    uint16_t getservoval();
-    void printf(char* str);
-    
-  private:
-    char getc_wait();
-    char getc_nowait();
-    
-  private:
-    Serial* serial;
-};
-
-#endif
\ No newline at end of file

--- a/main.cpp	Wed Sep 04 03:59:40 2013 +0000
+++ b/main.cpp	Fri Sep 06 08:36:21 2013 +0000
@@ -13,6 +13,6 @@
     
     while (1) {
         controlor.control();
-        wait(0.01);
+        wait(0.005);
     }
 }
\ No newline at end of file