Diff: main.cpp

Revision:

1:eabd95ed3f53

Parent:

0:5e49c3bc05f5

Child:

2:095a45ed34c5

diff -r 5e49c3bc05f5 -r eabd95ed3f53 main.cpp
--- a/main.cpp	Wed May 11 16:58:41 2016 +0000
+++ b/main.cpp	Thu May 19 15:06:47 2016 +0000
@@ -10,10 +10,17 @@
 
 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);                                        // brightness of PDU
+PwmOut brightness(PTA5);                                        // output to external pdu contols PMM level for brightness
+
 DigitalIn sw0(PTC9);
 DigitalIn sw1(PTC8);
 DigitalIn sw2(PTA12);
@@ -29,14 +36,15 @@
     output->putData(temp & 0xFFFFFF);
 };
 
-void GetABSPOS(Arguments *input, Reply *output)
+/*void GetABSPOS(Arguments *input, Reply *output)
 {
     led_green = !led_green;
-    // uint32_t temp = motor.GetParameter(ABS_POS);
+    // uint32_t temp = motor.(ABS_POS);     // rpc will not except uint32_t by default
     int temp = motor.GetParameter(ABS_POS);
-    temp = (temp & 0x3FFFFF);   // mask 22 bit value
+    temp = (temp & 0x3FFFFF);
     output->putData(temp);
 };
+*/
 
 // motor.move(direction, step)
 void Move(Arguments *input, Reply *output)
@@ -86,16 +94,23 @@
 void GetParameter(Arguments *input, Reply *output)
 {
     led_green = !led_green;
-    //uint8_t arg0 = input->getArg<int>();
-    //uint32_t temp = motor.GetParameter(arg0);
+    //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 variable and function assignments
+// RPC function assignments
 RPCFunction rpc_GetStatus(&GetStatus, "GetStatus");
-RPCFunction rpc_GetABSPOS(&GetABSPOS, "GetABSPOS");
+//RPCFunction rpc_GetABSPOS(&GetABSPOS, "GetABSPOS");
 RPCFunction rpc_Move(&Move, "Move");
 RPCFunction rpc_Run(&Run, "Run");
 RPCFunction rpc_Stop(&Stop, "Stop");
@@ -113,21 +128,118 @@
     motor.HardStop();
     motor.SetParameter(ACC, 0xFF);
     motor.SetParameter(DEC, 0xFF);
-    motor.SetParameter(MAX_SPEED, 0x17);
+    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, 0x30);
+    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.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();
     
     while(1){
-        if (sw0)
-          led_red = 0;
+        //status = motor.GetStatus();
+        position = motor.GetPosition();
+        if (position < openMotorPosition)
+            position = lastPosition;
+        if (position > closeMotorPosition)
+            position = lastPosition;
+        lastPosition = position;
+        
+        uint32_t p = (position - openMotorPosition)/motorPercent;
+        hc595.bar(p/10+1);
+
+        // check for auto mode
+        if (!sw0){
+            hc595.auto_led(true);
+            mode = 1;   // automatic mode
+        }
         else
-          led_red = 1;
+            hc595.auto_led(false);
+
+        // check for manual mode
+        if (!sw1){
+            hc595.manual_led(true);
+            mode = 0;
+        }
+        else
+            hc595.manual_led(false);
+
+        // 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);
+            }
+
+        // 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);
         
-        wait_ms(500);
     }
-}
+}
\ No newline at end of file