Diff: main.cpp

Revision:

0:0eddebdc2c58

Child:

1:3663d0f20136

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Aug 03 06:51:15 2017 +0000
@@ -0,0 +1,219 @@
+#include "mbed.h"
+#include "SPM.h"
+#include "trapezoidalMotionCal.h"
+
+//stepMotor x_axis(PC_4  ,PB_13 ,PB_14 ,PB_15);
+stepMotor x_axis(PB_15 ,PB_14 ,PB_13 ,PC_4);
+stepMotor y_axis(PB_1  ,PB_2  ,PB_12 ,PA_11);
+
+trapezoidalMotionCal motionCal(200, 200, 4000, 40000, -40000);
+
+PwmOut servo(PC_6);
+
+DigitalIn x_originSW(PA_0);
+DigitalIn y_originSW(PA_1);
+
+DigitalOut myled(LED1);
+Serial pc(USBTX,USBRX);
+
+typedef struct {
+    int32_t x;
+    int32_t y;
+} coordinate_t;
+
+int task = 0;
+int32_t pulse = 0;
+
+
+
+void pc_intr();
+void driveServo(int deg);
+void linearInterpolation(coordinate_t cStart, coordinate_t cTarg);
+void trapezoid(int32_t targSteps);
+
+// 受信割込みハンドラ
+void pc_intr()
+{
+    char temp = pc.getc();
+
+    switch(temp) {
+
+        case ',' :
+            if(task==0) {
+                task=1;
+                pulse=200;
+            }
+            break;
+        case '.' :
+            if(task==0) {
+                task=1;
+                pulse=-200;
+            }
+            break;
+        case 's' :
+            driveServo(120);
+            break;
+        case 'u' :
+            driveServo(100);
+            break;
+        case 'd' :
+            driveServo(60);
+            break;
+
+    }
+}
+
+void driveServo(int deg){
+    const static int servo_offset   =830;
+    const static float servo_gain   =10.0;
+    
+    deg=deg*servo_gain+servo_offset;
+    servo.pulsewidth_us(deg);
+    
+    return;
+}
+
+void itp(int32_t x, int32_t y){
+    static coordinate_t cPast = {0,0};
+    coordinate_t cCurrent = {x,y};
+    
+    linearInterpolation(cPast, cCurrent);
+    
+    cPast.x = x;
+    cPast.y = y;
+}
+
+int main()
+{
+    pc.attach(pc_intr, Serial::RxIrq);
+
+    pc.printf("Hello\r\n");
+    /*
+    x_axis.lock();
+    y_axis.lock();
+    */
+    driveServo(120);
+    
+    while(1) {
+
+        if(task==1) {
+
+            myled = 1;
+            
+            itp(400,  0);
+            wait_ms(10);
+            itp(400,400);
+            wait_ms(10);
+            itp(  0,400);
+            wait_ms(10);
+            itp(  0,  0);
+            wait_ms(10);
+            itp(400,  0);
+            wait_ms(10);
+            itp(  0,400);
+            wait_ms(10);
+            itp(400,400);
+            wait_ms(10);
+            itp(  0,  0);
+            wait_ms(10);
+            
+            myled = 0;
+
+            task=0;
+        }
+
+    }
+}
+
+void linearInterpolation(coordinate_t cStart, coordinate_t cTarg)
+{
+    coordinate_t delta;
+    int32_t error = 0;
+    bool x_inv = false;
+    bool y_inv = false;
+    coordinate_t cCurrent = {cStart.x, cStart.y};
+    coordinate_t inc = {0, 0};
+    uint32_t period_us = 0;
+    
+    if(cStart.x > cTarg.x) {
+        x_inv = true;
+        cTarg.x = cStart.x * 2 - cTarg.x;
+    }
+    if(cStart.y > cTarg.y) {
+        y_inv = true;
+        cTarg.y = cStart.y * 2 - cTarg.y;
+    }
+
+    delta.x = cTarg.x - cStart.x;
+    delta.y = cTarg.y - cStart.y;
+    
+    if(delta.x < delta.y){
+        motionCal.setTarg(delta.y);
+    }
+    else{
+        motionCal.setTarg(delta.x);
+    }
+    
+    while(true) {
+        inc.x = 0;
+        inc.y = 0;
+        if(delta.y > delta.x) {
+            error += delta.x;
+            if(error > delta.y) {
+                error -= delta.y;
+                cCurrent.x++;
+                inc.x = 1;
+            }
+            if(cCurrent.y == cTarg.y)break;
+            cCurrent.y++;
+            inc.y = 1;
+            period_us = motionCal.calStepDelay(cCurrent.y - cStart.y);
+        } else {
+            error += delta.y;
+            if(error > delta.x) {
+                error -= delta.x;
+                cCurrent.y++;
+                inc.y = 1;
+            }
+            if(cCurrent.x == cTarg.x)break;
+            cCurrent.x++;
+            inc.x = 1;
+            period_us = motionCal.calStepDelay(cCurrent.x - cStart.x);
+        }
+        
+        if(inc.x != 0)x_axis.oneStep(x_inv == 0);
+        if(inc.y != 0)y_axis.oneStep(y_inv == 0);
+        
+        wait_us(period_us);
+                
+    }
+
+}
+
+void trapezoid(int32_t targSteps)
+{
+    int revMode = 0;
+
+    uint32_t period_us = 0;
+    uint32_t steps = 0;
+
+    if(targSteps < 0) {
+        revMode = 1;
+        targSteps *= -1;
+    }
+    
+    motionCal.setTarg(targSteps);
+
+    while (steps <= targSteps) {
+
+        period_us = motionCal.calStepDelay(steps);
+
+        steps += 1;
+
+        x_axis.oneStep(revMode != 1);
+
+        wait_us(period_us);
+
+    }
+
+}