Steven Cheldelin / Mbed 2 deprecated STM32F303_GAMI_eMPC_motor_tester_v1

Stepper motor tester version 1 for the STM32F303K8 Nucleo development board

Dependencies:   RPCInterface mbed-dsp mbed

main.cpp@1:eabd95ed3f53, 2016-05-19 (annotated)

Committer:
rcflyair
Date:
Thu May 19 15:06:47 2016 +0000
Revision:
1:eabd95ed3f53
Parent:
0:5e49c3bc05f5
Child:
2:095a45ed34c5
commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rcflyair 0:5e49c3bc05f5 1 #include "mbed.h"
rcflyair 0:5e49c3bc05f5 2 #include "SerialRPCInterface.h"
rcflyair 0:5e49c3bc05f5 3 #include "L6470.h"
rcflyair 0:5e49c3bc05f5 4 #include "HC595.h"
rcflyair 0:5e49c3bc05f5 5
rcflyair 0:5e49c3bc05f5 6 // Global objects
rcflyair 0:5e49c3bc05f5 7 DigitalOut led_red(LED_RED); // active low to turn on red part of RGB LED on FRDM-K22F
rcflyair 0:5e49c3bc05f5 8 DigitalOut led_green(LED_GREEN); // active low to turn on green part of RGB LED on FRDM-K22F
rcflyair 0:5e49c3bc05f5 9 DigitalOut led_blue(LED_BLUE); // active low to turn on blue part of RGB LED on FRDM-K22F
rcflyair 0:5e49c3bc05f5 10
rcflyair 0:5e49c3bc05f5 11 SerialRPCInterface rpc(USBTX, USBRX, 115200); // create serial rpc interface on usb virtual com port
rcflyair 0:5e49c3bc05f5 12 L6470 motor(PTD6, PTD7, PTD5, PTD4); // create motor object with mosi, miso, sclk, cs on FRDM-K22F
rcflyair 1:eabd95ed3f53 13 int position; // current motor position from L6470 in steps
rcflyair 1:eabd95ed3f53 14 int mode; // current mode is either manual or auto
rcflyair 1:eabd95ed3f53 15 int range;
rcflyair 1:eabd95ed3f53 16 int closeMotorPosition;
rcflyair 1:eabd95ed3f53 17 int openMotorPosition;
rcflyair 1:eabd95ed3f53 18 int lastPosition;
rcflyair 0:5e49c3bc05f5 19
rcflyair 0:5e49c3bc05f5 20 HC595 hc595(PTC5, PTC10, PTC7); // create led bar driver with ser, oe, rclk, and srclk
rcflyair 0:5e49c3bc05f5 21 AnalogIn lightSense(PTB0); // analog voltage from light sensor on PDU
rcflyair 1:eabd95ed3f53 22 PwmOut brightness(PTA5); // output to external pdu contols PMM level for brightness
rcflyair 1:eabd95ed3f53 23
rcflyair 0:5e49c3bc05f5 24 DigitalIn sw0(PTC9);
rcflyair 0:5e49c3bc05f5 25 DigitalIn sw1(PTC8);
rcflyair 0:5e49c3bc05f5 26 DigitalIn sw2(PTA12);
rcflyair 0:5e49c3bc05f5 27 DigitalIn sw3(PTA13);
rcflyair 0:5e49c3bc05f5 28
rcflyair 0:5e49c3bc05f5 29 float ledbar = 1;
rcflyair 0:5e49c3bc05f5 30
rcflyair 0:5e49c3bc05f5 31 // RPC Function calls
rcflyair 0:5e49c3bc05f5 32 void GetStatus(Arguments *input, Reply *output)
rcflyair 0:5e49c3bc05f5 33 {
rcflyair 0:5e49c3bc05f5 34 led_green = !led_green;
rcflyair 0:5e49c3bc05f5 35 int temp = motor.GetStatus();
rcflyair 0:5e49c3bc05f5 36 output->putData(temp & 0xFFFFFF);
rcflyair 0:5e49c3bc05f5 37 };
rcflyair 0:5e49c3bc05f5 38
rcflyair 1:eabd95ed3f53 39 /*void GetABSPOS(Arguments *input, Reply *output)
rcflyair 0:5e49c3bc05f5 40 {
rcflyair 0:5e49c3bc05f5 41 led_green = !led_green;
rcflyair 1:eabd95ed3f53 42 // uint32_t temp = motor.(ABS_POS); // rpc will not except uint32_t by default
rcflyair 0:5e49c3bc05f5 43 int temp = motor.GetParameter(ABS_POS);
rcflyair 1:eabd95ed3f53 44 temp = (temp & 0x3FFFFF);
rcflyair 0:5e49c3bc05f5 45 output->putData(temp);
rcflyair 0:5e49c3bc05f5 46 };
rcflyair 1:eabd95ed3f53 47 */
rcflyair 0:5e49c3bc05f5 48
rcflyair 0:5e49c3bc05f5 49 // motor.move(direction, step)
rcflyair 0:5e49c3bc05f5 50 void Move(Arguments *input, Reply *output)
rcflyair 0:5e49c3bc05f5 51 {
rcflyair 0:5e49c3bc05f5 52 led_green = !led_green;
rcflyair 0:5e49c3bc05f5 53 int arg0 = input->getArg<int>();
rcflyair 0:5e49c3bc05f5 54 int arg1 = input->getArg<int>();
rcflyair 0:5e49c3bc05f5 55 motor.Move(arg0, arg1);
rcflyair 0:5e49c3bc05f5 56 };
rcflyair 0:5e49c3bc05f5 57
rcflyair 0:5e49c3bc05f5 58 void Run(Arguments *input, Reply *output)
rcflyair 0:5e49c3bc05f5 59 {
rcflyair 0:5e49c3bc05f5 60 led_green = !led_green;
rcflyair 0:5e49c3bc05f5 61 int arg0 = input->getArg<int>();
rcflyair 0:5e49c3bc05f5 62 motor.Run(arg0);
rcflyair 0:5e49c3bc05f5 63 };
rcflyair 0:5e49c3bc05f5 64 void Stop(Arguments *input, Reply *output)
rcflyair 0:5e49c3bc05f5 65 {
rcflyair 0:5e49c3bc05f5 66 led_green = !led_green;
rcflyair 0:5e49c3bc05f5 67 motor.HardStop();
rcflyair 0:5e49c3bc05f5 68 };
rcflyair 0:5e49c3bc05f5 69
rcflyair 0:5e49c3bc05f5 70 void ResetPosition(Arguments *input, Reply *output)
rcflyair 0:5e49c3bc05f5 71 {
rcflyair 0:5e49c3bc05f5 72 led_green = !led_green;
rcflyair 0:5e49c3bc05f5 73 motor.ResetPosition();
rcflyair 0:5e49c3bc05f5 74 };
rcflyair 0:5e49c3bc05f5 75
rcflyair 0:5e49c3bc05f5 76 void SetParameter(Arguments *input, Reply *output)
rcflyair 0:5e49c3bc05f5 77 {
rcflyair 0:5e49c3bc05f5 78 led_green = !led_green;
rcflyair 0:5e49c3bc05f5 79 // uint8_t arg0 = input->getArg<int>();
rcflyair 0:5e49c3bc05f5 80 // uint32_t arg1 = input->getArg<int>();
rcflyair 0:5e49c3bc05f5 81 int arg0 = input->getArg<int>();
rcflyair 0:5e49c3bc05f5 82 int arg1 = input->getArg<int>();
rcflyair 0:5e49c3bc05f5 83 motor.SetParameter(arg0, arg1);
rcflyair 0:5e49c3bc05f5 84 };
rcflyair 0:5e49c3bc05f5 85
rcflyair 0:5e49c3bc05f5 86 void SetLEDBar(Arguments *input, Reply *output)
rcflyair 0:5e49c3bc05f5 87 {
rcflyair 0:5e49c3bc05f5 88 led_green = !led_green;
rcflyair 0:5e49c3bc05f5 89 int arg0 = input->getArg<int>();
rcflyair 0:5e49c3bc05f5 90 hc595.bar(arg0);
rcflyair 0:5e49c3bc05f5 91 //output->putData(arg0);
rcflyair 0:5e49c3bc05f5 92 }
rcflyair 0:5e49c3bc05f5 93
rcflyair 0:5e49c3bc05f5 94 void GetParameter(Arguments *input, Reply *output)
rcflyair 0:5e49c3bc05f5 95 {
rcflyair 0:5e49c3bc05f5 96 led_green = !led_green;
rcflyair 1:eabd95ed3f53 97 //uint8_t arg0 = input->getArg<int>(); // rpc will not accept uint8_t by default
rcflyair 1:eabd95ed3f53 98 //uint32_t temp = motor.GetParameter(arg0); // rpc will not accept uint32_t by default
rcflyair 0:5e49c3bc05f5 99 int arg0 = input->getArg<int>();
rcflyair 0:5e49c3bc05f5 100 int temp = motor.GetParameter(arg0);
rcflyair 0:5e49c3bc05f5 101 output->putData(temp);
rcflyair 0:5e49c3bc05f5 102 }
rcflyair 1:eabd95ed3f53 103 // RPC variable assignments
rcflyair 1:eabd95ed3f53 104 RPCVariable<int> rpc_position(&position, "position");
rcflyair 1:eabd95ed3f53 105 RPCVariable<int> rpc_mode(&mode, "mode");
rcflyair 1:eabd95ed3f53 106 RPCVariable<int> rpc_range(&range, "range");
rcflyair 1:eabd95ed3f53 107 RPCVariable<int> rpc_openMotorPosition(&openMotorPosition, "openMotorPosition");
rcflyair 1:eabd95ed3f53 108 RPCVariable<int> rpc_closeMotorPostion(&closeMotorPosition, "closeMotorPosition");
rcflyair 1:eabd95ed3f53 109 RPCVariable<int> rpc_lastPosition(&lastPosition, "lastPosition");
rcflyair 0:5e49c3bc05f5 110
rcflyair 1:eabd95ed3f53 111 // RPC function assignments
rcflyair 0:5e49c3bc05f5 112 RPCFunction rpc_GetStatus(&GetStatus, "GetStatus");
rcflyair 1:eabd95ed3f53 113 //RPCFunction rpc_GetABSPOS(&GetABSPOS, "GetABSPOS");
rcflyair 0:5e49c3bc05f5 114 RPCFunction rpc_Move(&Move, "Move");
rcflyair 0:5e49c3bc05f5 115 RPCFunction rpc_Run(&Run, "Run");
rcflyair 0:5e49c3bc05f5 116 RPCFunction rpc_Stop(&Stop, "Stop");
rcflyair 0:5e49c3bc05f5 117 RPCFunction rpc_ResetPosition(&ResetPosition, "ResetPosition");
rcflyair 0:5e49c3bc05f5 118 RPCFunction rpc_SetParameter(&SetParameter, "SetParameter");
rcflyair 0:5e49c3bc05f5 119 RPCFunction rpc_GetParameter(&GetParameter, "GetParameter");
rcflyair 0:5e49c3bc05f5 120 RPCFunction rpc_SetLEDBar(&SetLEDBar, "SetLEDBar");
rcflyair 0:5e49c3bc05f5 121
rcflyair 0:5e49c3bc05f5 122 int main(){
rcflyair 0:5e49c3bc05f5 123 // Turn off LED's
rcflyair 0:5e49c3bc05f5 124 led_red = 1; led_green = 1; led_blue = 1;
rcflyair 0:5e49c3bc05f5 125
rcflyair 0:5e49c3bc05f5 126 // set motor parameters
rcflyair 0:5e49c3bc05f5 127 motor.Reset();
rcflyair 0:5e49c3bc05f5 128 motor.HardStop();
rcflyair 0:5e49c3bc05f5 129 motor.SetParameter(ACC, 0xFF);
rcflyair 0:5e49c3bc05f5 130 motor.SetParameter(DEC, 0xFF);
rcflyair 1:eabd95ed3f53 131 motor.SetParameter(MAX_SPEED, 0x20);
rcflyair 0:5e49c3bc05f5 132 motor.SetParameter(MIN_SPEED, 0x00);
rcflyair 0:5e49c3bc05f5 133 motor.SetParameter(KVAL_HOLD, 0x29);
rcflyair 0:5e49c3bc05f5 134 motor.SetParameter(KVAL_RUN, 0x29);
rcflyair 0:5e49c3bc05f5 135 motor.SetParameter(KVAL_ACC, 0x29);
rcflyair 0:5e49c3bc05f5 136 motor.SetParameter(KVAL_DEC, 0x29);
rcflyair 0:5e49c3bc05f5 137
rcflyair 1:eabd95ed3f53 138 motor.SetParameter(STALL_TH, 0x18);
rcflyair 1:eabd95ed3f53 139
rcflyair 1:eabd95ed3f53 140 // move motor OPEN to find motor limit with overcurrent detection
rcflyair 1:eabd95ed3f53 141 int status = 0;
rcflyair 1:eabd95ed3f53 142 int stall = 0;
rcflyair 1:eabd95ed3f53 143 int FindMotorLimitsCounter = 0;
rcflyair 1:eabd95ed3f53 144 motor.Run(OPEN);
rcflyair 1:eabd95ed3f53 145 // poll the motor status register for stall
rcflyair 1:eabd95ed3f53 146 // increment counter for timeout detection
rcflyair 1:eabd95ed3f53 147 while ((!stall) & (FindMotorLimitsCounter < 80)){
rcflyair 1:eabd95ed3f53 148 status = motor.GetStatus(); // read status register
rcflyair 1:eabd95ed3f53 149 stall = !((status >> 13) & 0x01) || !((status >> 14) & 0x01); // check for motor stall
rcflyair 1:eabd95ed3f53 150 FindMotorLimitsCounter++; // increment the timeout counter
rcflyair 1:eabd95ed3f53 151 wait_ms(100);
rcflyair 1:eabd95ed3f53 152 }
rcflyair 1:eabd95ed3f53 153 motor.HardStop(); // either motor stall or timeout has occured, so stop motor
rcflyair 1:eabd95ed3f53 154 status = motor.GetStatus(); // clear status register by reading
rcflyair 1:eabd95ed3f53 155
rcflyair 1:eabd95ed3f53 156 // if we have timed out, then flash red led forever (halt)
rcflyair 1:eabd95ed3f53 157 if (FindMotorLimitsCounter >= 80){
rcflyair 1:eabd95ed3f53 158 while(1){
rcflyair 1:eabd95ed3f53 159 led_green = 1; led_blue = 1; led_red = !led_red;
rcflyair 1:eabd95ed3f53 160 wait_ms(100);
rcflyair 1:eabd95ed3f53 161 }
rcflyair 1:eabd95ed3f53 162 }
rcflyair 1:eabd95ed3f53 163
rcflyair 1:eabd95ed3f53 164 // reset the motor position
rcflyair 1:eabd95ed3f53 165 motor.ResetPosition();
rcflyair 1:eabd95ed3f53 166
rcflyair 1:eabd95ed3f53 167 // store position of motor fully open - should be zero
rcflyair 1:eabd95ed3f53 168 openMotorPosition = motor.GetPosition();
rcflyair 1:eabd95ed3f53 169
rcflyair 1:eabd95ed3f53 170 // move motor CLOSE to find motor limit with overcurrent detection
rcflyair 1:eabd95ed3f53 171 status = 0;
rcflyair 1:eabd95ed3f53 172 stall = 0;
rcflyair 1:eabd95ed3f53 173 FindMotorLimitsCounter = 0;
rcflyair 1:eabd95ed3f53 174 //motor.SetParameter(STALL_TH, 0x18); // only used if close stall threshold is different than open threshold
rcflyair 1:eabd95ed3f53 175 motor.Run(CLOSE);
rcflyair 1:eabd95ed3f53 176 // poll the motor status register for stall
rcflyair 1:eabd95ed3f53 177 // increment counter for timeout detection
rcflyair 1:eabd95ed3f53 178 while ((!stall) & (FindMotorLimitsCounter < 80)){
rcflyair 1:eabd95ed3f53 179 status = motor.GetStatus(); // read the status register
rcflyair 1:eabd95ed3f53 180 stall = !((status >> 13) & 0x01) || !((status >> 14) & 0x01); // check for motor stall
rcflyair 1:eabd95ed3f53 181 FindMotorLimitsCounter++; // increment the timeout counter
rcflyair 1:eabd95ed3f53 182 wait_ms(100); // poll loop delay
rcflyair 1:eabd95ed3f53 183 }
rcflyair 1:eabd95ed3f53 184 motor.HardStop();
rcflyair 1:eabd95ed3f53 185 status = motor.GetStatus();
rcflyair 1:eabd95ed3f53 186 if (FindMotorLimitsCounter >= 80){ // if has not stalled yet, display flashing red as error
rcflyair 1:eabd95ed3f53 187 while(1){
rcflyair 1:eabd95ed3f53 188 led_green = !led_green; led_blue = 1; led_red = 1;
rcflyair 1:eabd95ed3f53 189 wait_ms(100);
rcflyair 1:eabd95ed3f53 190 }
rcflyair 1:eabd95ed3f53 191 }
rcflyair 1:eabd95ed3f53 192 led_green = 1; led_blue = 1; led_red = 1;
rcflyair 1:eabd95ed3f53 193 // store position of motor fully close
rcflyair 1:eabd95ed3f53 194 //** hardcore test rpc connection **\\ closeMotorPosition = 200000;//motor.GetParameter(ABS_POS);
rcflyair 1:eabd95ed3f53 195 closeMotorPosition = motor.GetPosition();
rcflyair 1:eabd95ed3f53 196
rcflyair 1:eabd95ed3f53 197 // calculate motor movement range
rcflyair 1:eabd95ed3f53 198 range = closeMotorPosition - openMotorPosition;
rcflyair 1:eabd95ed3f53 199 float motorPercent = range / 100;
rcflyair 1:eabd95ed3f53 200 motor.Move(OPEN, range);
rcflyair 1:eabd95ed3f53 201 lastPosition = motor.GetPosition();
rcflyair 0:5e49c3bc05f5 202
rcflyair 0:5e49c3bc05f5 203 while(1){
rcflyair 1:eabd95ed3f53 204 //status = motor.GetStatus();
rcflyair 1:eabd95ed3f53 205 position = motor.GetPosition();
rcflyair 1:eabd95ed3f53 206 if (position < openMotorPosition)
rcflyair 1:eabd95ed3f53 207 position = lastPosition;
rcflyair 1:eabd95ed3f53 208 if (position > closeMotorPosition)
rcflyair 1:eabd95ed3f53 209 position = lastPosition;
rcflyair 1:eabd95ed3f53 210 lastPosition = position;
rcflyair 1:eabd95ed3f53 211
rcflyair 1:eabd95ed3f53 212 uint32_t p = (position - openMotorPosition)/motorPercent;
rcflyair 1:eabd95ed3f53 213 hc595.bar(p/10+1);
rcflyair 1:eabd95ed3f53 214
rcflyair 1:eabd95ed3f53 215 // check for auto mode
rcflyair 1:eabd95ed3f53 216 if (!sw0){
rcflyair 1:eabd95ed3f53 217 hc595.auto_led(true);
rcflyair 1:eabd95ed3f53 218 mode = 1; // automatic mode
rcflyair 1:eabd95ed3f53 219 }
rcflyair 0:5e49c3bc05f5 220 else
rcflyair 1:eabd95ed3f53 221 hc595.auto_led(false);
rcflyair 1:eabd95ed3f53 222
rcflyair 1:eabd95ed3f53 223 // check for manual mode
rcflyair 1:eabd95ed3f53 224 if (!sw1){
rcflyair 1:eabd95ed3f53 225 hc595.manual_led(true);
rcflyair 1:eabd95ed3f53 226 mode = 0;
rcflyair 1:eabd95ed3f53 227 }
rcflyair 1:eabd95ed3f53 228 else
rcflyair 1:eabd95ed3f53 229 hc595.manual_led(false);
rcflyair 1:eabd95ed3f53 230
rcflyair 1:eabd95ed3f53 231 // manual mode and position switch requests close
rcflyair 1:eabd95ed3f53 232 if ((!sw2)&&(!sw1)) {
rcflyair 1:eabd95ed3f53 233 if (position + 1000 < closeMotorPosition) motor.Move(CLOSE, 1000);
rcflyair 1:eabd95ed3f53 234 else if ((closeMotorPosition - position) > 0) motor.Move(CLOSE, closeMotorPosition - position);
rcflyair 1:eabd95ed3f53 235 }
rcflyair 1:eabd95ed3f53 236
rcflyair 1:eabd95ed3f53 237 // manual mode and position switch requests open
rcflyair 1:eabd95ed3f53 238 if ((!sw3)&&(!sw1)) {
rcflyair 1:eabd95ed3f53 239 if (position - 1000 > 0) motor.Move(OPEN, 1000);
rcflyair 1:eabd95ed3f53 240 else if (position > 0) motor.Move(OPEN, position);
rcflyair 1:eabd95ed3f53 241 }
rcflyair 1:eabd95ed3f53 242 wait_ms(100);
rcflyair 0:5e49c3bc05f5 243
rcflyair 0:5e49c3bc05f5 244 }
rcflyair 1:eabd95ed3f53 245 }