Diff: motor/motor.cpp

Revision:

2:856f03e64695

Child:

3:2194b5b4f82a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor/motor.cpp	Thu Nov 09 03:40:43 2017 +0000
@@ -0,0 +1,135 @@
+#include "motor.h"
+
+Motor::Motor(PinName p, PinName d, PinName e, char* n, float Tcs, int cw):
+    pulse(p),
+    dir(d),
+    error(e),
+    TXcm_s(Tcs),
+    name(n),
+    cw(cw),
+    ccw(1-cw),
+    lock(false)
+{
+    pulse = 0;
+    dir = 0;
+    error = 1;
+    pos = 0;
+    setSpeed(1);
+}
+
+void Motor::pwm_io(int p_us, float dc)
+{
+    timer.detach();
+    if ((p_us == 0) || (dc == 0)) {
+        return;
+    }
+    if (dc >= 1) {
+        return;
+    }
+    pulse = 0;
+    dir = 0;
+    error = 1;
+    on_delay = (int)(p_us * dc);
+    off_delay = p_us - on_delay;
+    printf("on delay: %dus\r\noff_delay: %dus\r\n", on_delay, off_delay);
+    toggleOn();
+}
+
+void Motor::toggleOn(void)
+{
+    pulse = 1;
+    timer.attach_us(this, &Motor::toggleOff, on_delay);
+}
+
+void Motor::toggleOff(void)
+{
+    pulse = 0;
+    timer.attach_us(this, &Motor::toggleOn, off_delay);
+}
+
+void Motor::reach_goal(void)
+{
+    timer.detach();
+    pos=goal;
+    lock=false;
+}
+void Motor::go(int direction)
+{
+    if(lock) {
+        return;
+    }
+    lock=true;
+    timer.detach();
+    pulse = 0;
+    dir = (direction?ccw:cw);
+    error = 1;
+    wait(0.001);
+    toggleOn();
+    wait(0.1);
+    float d=speed()*0.1;
+    if(direction == UP) pos += d;
+    else pos -= d;
+    timer.detach();
+    lock=false;
+}
+
+void Motor::to(float p)
+{
+    if(lock) {
+        return;
+    }
+    if (p==pos) return;
+    lock=true;
+    float time=(p-pos)/speed();
+    int direction=(time>0)?UP:DOWN;
+    if(time<0) time=-time;
+    timer.detach();
+    pulse = 0;
+    dir = (direction?ccw:cw);
+    error = 1;
+    wait(0.001);
+    toggleOn();
+    
+    wait(time);
+    timer.detach();
+    pos=p;
+    lock=false;
+//    goal=p;
+//    timer.attach(this, &Motor::reach_goal, time);
+}
+
+void Motor::setSpeed(float cm_s)
+{
+    if(lock) {
+        return;
+    }
+    lock=true;
+    float T=TXcm_s/cm_s;
+    on_delay=(int)T/2;
+    off_delay=T-on_delay;
+    lock=false;
+}
+
+CD_2D34M::CD_2D34M(char* n, PinName d, PinName p, PinName a, PinName e):
+    Motor(p,d,e,n,CD_2D34M__TxCM_S,1), alarm(a)
+{
+#ifdef ALARM
+    alarm.fall(this, &CD_2D34M::alarm_event);
+#endif
+    //alarm.fall(this, &CD_2D34M::alarm_reset);
+}
+void CD_2D34M::alarm_event()
+{
+    error = 0;
+    wait(0.01);
+    alarm_reset();
+}
+void CD_2D34M::alarm_reset()
+{
+    error = 1;
+    wait(0.01);
+}
+
+CD_2D24MB::CD_2D24MB(char* n, PinName p, PinName d, PinName e):
+    Motor(p,d,e,n,CD_2D24MB__TxCM_S,0)
+{}
\ No newline at end of file