Diff: main.cpp

Revision:

2:095a45ed34c5

Parent:

1:eabd95ed3f53

--- a/main.cpp	Thu May 19 15:06:47 2016 +0000
+++ b/main.cpp	Tue Dec 20 21:40:49 2016 +0000
@@ -1,245 +1,147 @@
-#include "mbed.h"
-#include "SerialRPCInterface.h"
-#include "L6470.h"
-#include "HC595.h"
+// motor test firmware
+// STM32F303 version 1.0
+// 2016-12-19
+// Cheldelin
+
+#include "mbed.h"                                   // main mbed library
+#include "SerialRPCInterface.h"                     // mbed rpc library for serial rpc communication to Labview
+#include "dsp.h"                                    // mbed cmsis arm library for PID control
+#include "STM32F303_L6470_v1.h"                     // L6470 stepper motor conrol interface for ST
+
+#define OFF 0
+#define ON 1
 
 // Global objects
-DigitalOut led_red(LED_RED);                                    // active low to turn on red part of RGB LED on FRDM-K22F
-DigitalOut led_green(LED_GREEN);                                // active low to turn on green part of RGB LED on FRDM-K22F
-DigitalOut led_blue(LED_BLUE);                                  // active low to turn on blue part of RGB LED on FRDM-K22F
-
-SerialRPCInterface rpc(USBTX, USBRX, 115200);                   // create serial rpc interface on usb virtual com port
-L6470 motor(PTD6, PTD7, PTD5, PTD4);                            // create motor object with mosi, miso, sclk, cs on FRDM-K22F
-int position;                                                   // current motor position from L6470 in steps
-int mode;                                                       // current mode is either manual or auto
-int range;
-int closeMotorPosition;
-int openMotorPosition;
-int lastPosition;
-
-HC595 hc595(PTC5, PTC10, PTC7);                                 // create led bar driver with ser, oe, rclk, and srclk
-AnalogIn lightSense(PTB0);                                      // analog voltage from light sensor on PDU
-PwmOut brightness(PTA5);                                        // output to external pdu contols PMM level for brightness
-
-DigitalIn sw0(PTC9);
-DigitalIn sw1(PTC8);
-DigitalIn sw2(PTA12);
-DigitalIn sw3(PTA13);
-
-float ledbar = 1;
+DigitalOut led_green(PB_3);                         // active low to turn on green part of RGB LED on FRDM-K22F
 
-// RPC Function calls
-void GetStatus(Arguments *input, Reply *output)
-{
-    led_green = !led_green;
-    int temp = motor.GetStatus();
-    output->putData(temp & 0xFFFFFF);
-};
-
-/*void GetABSPOS(Arguments *input, Reply *output)
-{
-    led_green = !led_green;
-    // uint32_t temp = motor.(ABS_POS);     // rpc will not except uint32_t by default
-    int temp = motor.GetParameter(ABS_POS);
-    temp = (temp & 0x3FFFFF);
-    output->putData(temp);
-};
-*/
-
-// motor.move(direction, step)
-void Move(Arguments *input, Reply *output)
-{
-    led_green = !led_green;
-    int arg0 = input->getArg<int>();
-    int arg1 = input->getArg<int>();
-    motor.Move(arg0, arg1);
-};
+SerialRPCInterface rpc(USBTX, USBRX, 115200);       // create serial rpc interface on usb virtual com port
+L6470 motor(PA_7, PA_6, PA_5, PA_4);                // create motor object with mosi, miso, sclk, cs on NUCLEO STMF303
 
-void Run(Arguments *input, Reply *output)
-{
-    led_green = !led_green;
-    int arg0 = input->getArg<int>();
-    motor.Run(arg0);
-};
-void Stop(Arguments *input, Reply *output)
-{
-    led_green = !led_green;
-    motor.HardStop();
-};
-
-void ResetPosition(Arguments *input, Reply *output)
-{
-    led_green = !led_green;
-    motor.ResetPosition();
-};
-
-void SetParameter(Arguments *input, Reply *output)
-{
-    led_green = !led_green;
-    // uint8_t arg0 = input->getArg<int>();
-    // uint32_t arg1 = input->getArg<int>();
-    int arg0 = input->getArg<int>();
-    int arg1 = input->getArg<int>();
-    motor.SetParameter(arg0, arg1);
-};
+int   direction;                                    // global holds current direction to move motor, either OPEN or CLOSE
+float distance_1;                                   // global holds current distance to move motor in steps
+float distance_2;
+float position = 0;
 
-void SetLEDBar(Arguments *input, Reply *output)
-{
-    led_green = !led_green;
-    int arg0 = input->getArg<int>();
-    hc595.bar(arg0);
-    //output->putData(arg0);
-}
-
-void GetParameter(Arguments *input, Reply *output)
-{
-    led_green = !led_green;
-    //uint8_t arg0 = input->getArg<int>();      // rpc will not accept uint8_t by default
-    //uint32_t temp = motor.GetParameter(arg0); // rpc will not accept uint32_t by default
-    int arg0 = input->getArg<int>();
-    int temp = motor.GetParameter(arg0);
-    output->putData(temp);
-}
 // RPC variable assignments
-RPCVariable<int> rpc_position(&position, "position");
-RPCVariable<int> rpc_mode(&mode, "mode");
-RPCVariable<int> rpc_range(&range, "range");
-RPCVariable<int> rpc_openMotorPosition(&openMotorPosition, "openMotorPosition");
-RPCVariable<int> rpc_closeMotorPostion(&closeMotorPosition, "closeMotorPosition");
-RPCVariable<int> rpc_lastPosition(&lastPosition, "lastPosition");
-
-// RPC function assignments
-RPCFunction rpc_GetStatus(&GetStatus, "GetStatus");
-//RPCFunction rpc_GetABSPOS(&GetABSPOS, "GetABSPOS");
-RPCFunction rpc_Move(&Move, "Move");
-RPCFunction rpc_Run(&Run, "Run");
-RPCFunction rpc_Stop(&Stop, "Stop");
-RPCFunction rpc_ResetPosition(&ResetPosition, "ResetPosition");
-RPCFunction rpc_SetParameter(&SetParameter, "SetParameter");
-RPCFunction rpc_GetParameter(&GetParameter, "GetParameter");
-RPCFunction rpc_SetLEDBar(&SetLEDBar, "SetLEDBar");
+RPCVariable<int>   rpc_position(&motor.position, "position");
+RPCVariable<int>   rpc_range(&motor.range, "range");
+RPCVariable<int>   rpc_direction(&direction, "direction");
+RPCVariable<float> rpc_distance_1(&distance_1, "distance_1");
+RPCVariable<float> rpc_distance_2(&distance_2, "distance_2");
 
 int main(){
-    // Turn off LED's
-    led_red = 1; led_green = 1; led_blue = 1;
-    
-    // set motor parameters
-    motor.Reset();
+
+    // Turn off LED
+    led_green = OFF;
+
+    // delay to allow L6470 to power up
+    // TODO:  set up digital pin on BUSY and/or FLAG pin for feedback
+    wait_ms(1000);
+
+    // Set the default motor parameters
+    motor.SetDefaultParameters();
+
+    // troubleshooting???
     motor.HardStop();
-    motor.SetParameter(ACC, 0xFF);
-    motor.SetParameter(DEC, 0xFF);
-    motor.SetParameter(MAX_SPEED, 0x20);
-    motor.SetParameter(MIN_SPEED, 0x00);
-    motor.SetParameter(KVAL_HOLD, 0x29);
-    motor.SetParameter(KVAL_RUN, 0x29);
-    motor.SetParameter(KVAL_ACC, 0x29);
-    motor.SetParameter(KVAL_DEC, 0x29);
-    
-    motor.SetParameter(STALL_TH, 0x18);
-    
-    // move motor OPEN to find motor limit with overcurrent detection
-    int status = 0;
-    int stall = 0;
-    int FindMotorLimitsCounter = 0;
-    motor.Run(OPEN);
-    // poll the motor status register for stall
-    //  increment counter for timeout detection
-    while ((!stall) & (FindMotorLimitsCounter < 80)){
-        status = motor.GetStatus();                                         // read status register
-        stall = !((status >> 13) & 0x01) || !((status >> 14) & 0x01);       // check for motor stall
-        FindMotorLimitsCounter++;                                           // increment the timeout counter
-        wait_ms(100);
-    }    
-    motor.HardStop();                                                       // either motor stall or timeout has occured, so stop motor
-    status = motor.GetStatus();                                             // clear status register by reading
-    
-    // if we have timed out, then flash red led forever (halt)
-    if (FindMotorLimitsCounter >= 80){
-        while(1){
-            led_green = 1; led_blue = 1; led_red = !led_red;
-            wait_ms(100);
-            }
-        }
- 
-    // reset the motor position
+    motor.SoftHIZ();
+    wait_ms(1000);
+
+    // set step mode
+    uint32_t StepMode = motor.GetParameter(STEP_MODE);
+    StepMode = StepMode & 0xfffffff0;  // turn off microstepping
+    //StepMode = StepMode + 0x00; // new step mode 0x00 thru 0x07
+    motor.SetParameter(STEP_MODE, StepMode);   // set step mode
     motor.ResetPosition();
-        
-    // store position of motor fully open - should be zero
-    openMotorPosition = motor.GetPosition();
     
-    // move motor CLOSE to find motor limit with overcurrent detection
-    status = 0;
-    stall = 0;
-    FindMotorLimitsCounter = 0;
-    //motor.SetParameter(STALL_TH, 0x18);   // only used if close stall threshold is different than open threshold
-    motor.Run(CLOSE);
-    // poll the motor status register for stall
-    //  increment counter for timeout detection
-    while ((!stall) & (FindMotorLimitsCounter < 80)){
-        status = motor.GetStatus();                                         // read the status register
-        stall = !((status >> 13) & 0x01) || !((status >> 14) & 0x01);       // check for motor stall
-        FindMotorLimitsCounter++;                                           // increment the timeout counter
-        wait_ms(100);                                                       // poll loop delay
-    }    
-    motor.HardStop();
-    status = motor.GetStatus();
-    if (FindMotorLimitsCounter >= 80){                                      // if has not stalled yet, display flashing red as error
-        while(1){
-            led_green = !led_green; led_blue = 1; led_red = 1;
-            wait_ms(100);
-            }
-        }
-    led_green = 1; led_blue = 1; led_red = 1;
-    // store position of motor fully close
-    //** hardcore test rpc connection **\\     closeMotorPosition = 200000;//motor.GetParameter(ABS_POS);
-    closeMotorPosition = motor.GetPosition();
-
-    // calculate motor movement range
-    range = closeMotorPosition - openMotorPosition;
-    float motorPercent = range / 100;
-    motor.Move(OPEN, range);
-    lastPosition = motor.GetPosition();
-    
+    // Main loop
     while(1){
-        //status = motor.GetStatus();
-        position = motor.GetPosition();
-        if (position < openMotorPosition)
-            position = lastPosition;
-        if (position > closeMotorPosition)
-            position = lastPosition;
-        lastPosition = position;
+        uint16_t MaxSpeed = 0x6f;
+        uint16_t MaxTime = 1400;
         
-        uint32_t p = (position - openMotorPosition)/motorPercent;
-        hc595.bar(p/10+1);
+        // close full speed
+        led_green = ON;
+        motor.SetParameter(MAX_SPEED, MaxSpeed);
+        //motor.SpeedRun(CLOSE, 0x0010);
+        motor.Move(CLOSE, 1400);
+        wait_ms(MaxTime);
+
+        // open full speed
+        led_green = OFF;
+        motor.SetParameter(MAX_SPEED, MaxSpeed);
+        //motor.SpeedRun(CLOSE, 0x0004ff);
+        motor.Move(OPEN, 1400);
+        wait_ms(MaxTime);
 
-        // check for auto mode
-        if (!sw0){
-            hc595.auto_led(true);
-            mode = 1;   // automatic mode
-        }
-        else
-            hc595.auto_led(false);
+        // close half speed
+        led_green = ON;
+        motor.SetParameter(MAX_SPEED, MaxSpeed >> 1);
+        //motor.SpeedRun(CLOSE, 0x0010);
+        motor.Move(CLOSE, 1400);
+        wait_ms(MaxTime << 1);
+
+        // open half speed
+        led_green = OFF;
+        motor.SetParameter(MAX_SPEED, MaxSpeed >> 1);
+        //motor.SpeedRun(CLOSE, 0x0004ff);
+        motor.Move(OPEN, 1400);
+        wait_ms(MaxTime << 1);
+
+        // close quarter speed
+        led_green = ON;
+        motor.SetParameter(MAX_SPEED, MaxSpeed >> 2);
+        //motor.SpeedRun(CLOSE, 0x0010);
+        motor.Move(CLOSE, 1400);
+        wait_ms(MaxTime << 2);
 
-        // check for manual mode
-        if (!sw1){
-            hc595.manual_led(true);
-            mode = 0;
-        }
-        else
-            hc595.manual_led(false);
+        // open quarter speed
+        led_green = OFF;
+        motor.SetParameter(MAX_SPEED, MaxSpeed >> 2);
+        //motor.SpeedRun(CLOSE, 0x0004ff);
+        motor.Move(OPEN, 1400);
+        wait_ms(MaxTime << 2);
+
+        uint8_t KVal  = 0x05;
+        motor.SetLowSpeedOpt(OFF);          // turn of low speed optimization
+        // close eighth speed
+        led_green = ON;
+        motor.SetParameter(KVAL_RUN, KVal);
+        motor.SetParameter(MAX_SPEED, MaxSpeed >> 3);
+        //motor.SpeedRun(CLOSE, 0x0010);
+        motor.SetParameter(KVAL_RUN, 0x0F);   // increase motor drive at slowest speed
+        motor.Move(CLOSE, 1400);
+        motor.SetParameter(KVAL_RUN, KVal);   // set motor drive back to default
+        wait_ms(MaxTime << 3);
 
-        // manual mode and position switch requests close
-        if ((!sw2)&&(!sw1)) {
-            if (position + 1000 < closeMotorPosition) motor.Move(CLOSE, 1000);
-            else if ((closeMotorPosition - position) > 0) motor.Move(CLOSE, closeMotorPosition - position);
-            }
+        // open eighth speed
+        led_green = OFF;
+        motor.SetParameter(MAX_SPEED, MaxSpeed >> 3);   // increase motor drive at slowest speed
+        //motor.SpeedRun(CLOSE, 0x0004ff);
+        motor.SetParameter(KVAL_RUN, 0x0F);                   // set motor drive back to default
+        motor.Move(OPEN, 1400);
+        motor.SetParameter(KVAL_RUN, KVal);
+        wait_ms(MaxTime << 3);
 
-        // manual mode and position switch requests open
-        if ((!sw3)&&(!sw1)) {
-            if (position - 1000 > 0) motor.Move(OPEN, 1000);
-            else if (position > 0) motor.Move(OPEN, position);
-            }
-        wait_ms(100);
-        
+        led_green = ON;
+        KVal =  0x10;
+        motor.SetLowSpeedOpt(ON);          // turn of low speed optimization
+        // close eighth speed
+        motor.SetParameter(KVAL_RUN, KVal);
+        motor.SetParameter(MAX_SPEED, MaxSpeed >> 3);
+        //motor.SpeedRun(CLOSE, 0x0010);
+        motor.SetParameter(KVAL_RUN, 0x0F);   // increase motor drive at slowest speed
+        motor.Move(CLOSE, 1400);
+        motor.SetParameter(KVAL_RUN, KVal);   // set motor drive back to default
+        wait_ms(MaxTime << 3);
+
+        // open eighth speed
+        led_green = OFF;
+        motor.SetParameter(MAX_SPEED, MaxSpeed >> 3);   // increase motor drive at slowest speed
+        //motor.SpeedRun(CLOSE, 0x0004ff);
+        motor.SetParameter(KVAL_RUN, 0x0F);                   // set motor drive back to default
+        motor.Move(OPEN, 1400);
+        motor.SetParameter(KVAL_RUN, KVal);
+        wait_ms(MaxTime << 3);
+        led_green = 0;
+
     }
 }
\ No newline at end of file