Mircea Murar
SMC Axis control
Diff: main.cpp
- Revision:
- 0:bf14022d00d5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Apr 12 12:07:04 2016 +0000
@@ -0,0 +1,791 @@
+#include "mbed.h"
+#include "I2C.h"
+#include "SLCD.h"
+
+// v12
+// partea de control merge
+
+// intrarile de la controlerul SMC sunt inversate
+
+#define NO_POWER_SUPPLY 0x7F
+#define LECP6_IN_ALARM 0x60
+#define RETURN_TO_ORIGIN 0x02
+#define LECP6_SCAN_INPUTS 0x07
+#define LECP6_MOVES 0x08
+#define LECP6_DONE 0x09
+#define LECP6_SETUP 0x0A
+#define LECP6_JOG 0x15
+#define POSITION_OVERFLOW 0x11
+#define POSITION_UNDERFLOW 0x12
+#define POSITION_RECORDED 0x30
+#define ESC 0x40
+#define BUILD_CTRL 0x50
+#define CTRL_CREATED 0x51
+#define CTRL_RUN 0x52
+#define LECP6_RST 0xFF
+
+
+Ticker operating_led;
+Ticker equipment_status;
+
+// 4 digit LCD
+SLCD slcd;
+/*
+I2C I2C_Comm(PTC9,PTC8);
+I2C I2C_LCD(PTC2,PTC1);
+const int addr = 0x90;
+//Serial UART_Comm(PTA2, PTA1);
+*/
+DigitalOut LED(PTE29);
+DigitalOut Relay_CTRL(PTE31);
+DigitalOut HP_Output(PTB19);
+DigitalIn Board_But_Left(PTC12);
+DigitalIn Board_But_Right(PTC3);
+
+// MCU Digital Output to control Axis/Gripper
+/*
+DigitalOut DO_A3_IN0(PTE18);
+DigitalOut DO_A4_IN1(PTE17);
+DigitalOut DO_A5_IN2(PTE16);
+DigitalOut DO_A6_IN3(PTE6);
+DigitalOut DO_A7_IN4(PTE3);
+DigitalOut DO_A8_IN5(PTE2);
+*/
+DigitalOut DO_A9_SETUP(PTA14);
+DigitalOut DO_A10_HOLD(PTA6);
+DigitalOut DO_A11_DRIVE(PTA7);
+DigitalOut DO_A12_RESET(PTC16);
+DigitalOut DO_A13_SVON(PTC13);
+
+// MCU Digital Input for feedback from Axis/Gripper
+DigitalIn DI_B1_OUT0(PTE19);
+DigitalIn DI_B2_OUT1(PTE1);
+DigitalIn DI_B3_OUT2(PTD7);
+DigitalIn DI_B4_OUT3(PTA17);
+DigitalIn DI_B5_OUT4(PTA16);
+DigitalIn DI_B6_OUT5(PTA15);
+DigitalIn DI_B7_BUSY(PTA13);
+DigitalIn DI_B8_AREA(PTD2);
+DigitalIn DI_B9_SETON(PTD4);
+DigitalIn DI_B10_INP(PTD6);
+DigitalIn DI_B11_SVRE(PTD7);
+DigitalIn DI_B12_ESTOP(PTD5);
+DigitalIn DI_B13_ALARM(PTE0);
+
+BusOut step_number(PTE18,PTE17,PTE16,PTE6,PTE3,PTE2);
+BusIn equipment_active_step(PTE19, PTE1, PTD7, PTA17, PTA16, PTA15);
+BusIn equipment_system_status(PTA13, PTD2, PTD4, PTD6, PTD7, PTD5, PTE0);
+
+DigitalIn OPEN_CLOSE(PTB2);
+
+/*
+I2C ioLCD(PTC1,PTC2);
+DigitalIn HP_Input(PTB18);
+*/
+
+DigitalIn BUT_Verde(PTE30);
+DigitalIn BUT_Maro(PTB20);
+DigitalIn BUT_Portocaliu(PTE23);
+DigitalIn BUT_Rosu(PTB2);
+
+/*
+DigitalOut DO_1(PTA12);
+DigitalOut DO_2(PTA4);
+*/
+
+void config();
+void operating_flag();
+void LECP6_read_inputs();
+void axis_position();
+void showIndex_Inc(int);
+void showMessage(int);
+
+bool LECP_alarms();
+void LECP_alarms_rst();
+bool LECP6_SVRE();
+bool LECP6_BUSY();
+bool LECP_move_axis();
+bool LECP_return_to_origin();
+void LECP6_disable_outputs();
+void LECP_Jog();
+void LECP_Build_Control();
+void LECP_Run_Control();
+void LECP_Run(int*);
+
+
+bool equipment_ready = false, equip_in_alarm = false;
+
+int sys_Status, equip_active;
+int move_forward = 0, move_backward = 0;
+float memorised_positions[10];
+float control_sequences[10][10];
+int number_of_mem_positions = 0;
+float position;
+float positions[] = { 0.0, 0.1, 0.25, 0.5, 1.0, 2.5, 5.0, 10.0, 15.0, 20.0, 25.0, 30.0, 35.0, 40.0, 45.0, 50.0, 55.0, 60.0, 65.0, 70.0, 75.0, 80.0, 85.0, 90.0, 95.0, 100.0, 110.0, 120.0, 130.0, 140.0, 150.0,
+ -0.1,-0.25,-0.5,-1.0,-2.5,-5.0,-10.0,-15.0,-20.0,-25.0,-30.0,-35.0,-40.0,-45.0,-50.0,-55.0,-60.0,-65.0,-70.0,-75.0,-80.0,-85.0,-90.0,-95.0,-100.0,-110.0,-120.0,-130.0,-140.0,-150.0
+ };
+
+int main()
+{
+ config();
+
+ control_sequences[1][0] = 3;
+ control_sequences[1][1] = 20.0;
+ control_sequences[1][2] = 50.0;
+ control_sequences[1][3] = 25.0;
+ /* int contr[10];
+ contr[0] = 4;
+ contr[1] = 5;
+ contr[2] = 28;
+ contr[3] = 58;
+ contr[4] = 35;
+ */
+ //LECP_return_to_origin();
+ //LECP_Run(contr);
+
+ while(1) {
+
+ /******* RETURN TO ORIGIN ********/
+ if(sys_Status != NO_POWER_SUPPLY) {
+ if(!equip_in_alarm && (!equipment_ready || !Board_But_Left)) {
+ showMessage(RETURN_TO_ORIGIN);
+ equipment_ready = LECP_return_to_origin();
+ }
+ }
+
+ /********* JOGING ***********/
+ if(equipment_ready && !BUT_Portocaliu) {
+ showMessage(LECP6_JOG);
+ LECP_Jog();
+ }
+
+ /******** CONTROL ***********/
+ if(equipment_ready && !BUT_Maro) {
+ showMessage(BUILD_CTRL);
+ LECP_Build_Control();
+ }
+
+ /******** RUN CONTROL SEQUENCE ********/
+ if(equipment_ready && !BUT_Verde) {
+ showMessage(CTRL_RUN);
+ LECP_Run_Control();
+ }
+ }
+}
+
+
+
+void LECP_Run(int* ctrl)
+{
+
+ // in functia aceasta sa introduc si return to origin
+
+ int move_axis_task = 0, alarm = 0;
+ int last_step = ctrl[0], step;
+
+ while(BUT_Rosu && !equip_in_alarm && equipment_ready && !alarm) { // cat timp nu este oprita secventa de control
+ switch(move_axis_task) {
+ case 0:
+ // check if axis is not in fault or alarm
+ while(equip_in_alarm) {
+ }
+ move_axis_task++; // next task
+ break;
+
+ case 1:
+ DO_A13_SVON = true; // turn servo on;
+ if(LECP6_SVRE()) {
+ move_axis_task++; // next task
+ } else {
+ alarm = 1;
+ move_axis_task = 100;
+ }
+ break;
+
+ case 2:
+ for( step = 1; step < last_step && !equip_in_alarm; step++) {
+ step_number = ctrl[step];
+ if(step_number == 0 ) position = 0;
+ else position = position + positions[step_number];
+ DO_A11_DRIVE = true; // set drive bit
+ slcd.clear();
+ slcd.Home();
+ wait_ms(50);
+ DO_A11_DRIVE = false; // reset drive bit
+ slcd.printf("%.1f", position);
+ do {
+ wait_ms(10);
+ } while(DI_B10_INP);
+ do {
+ wait_ms(10);
+ } while(LECP6_BUSY());
+
+ }
+ if(!equip_in_alarm) move_axis_task = 0; // next task
+ else {
+ alarm = 2;
+ move_axis_task = 100;
+ }
+ break;
+
+ default:
+ break;
+ }
+ }
+ DO_A13_SVON = false; // turn servo off;
+
+}
+
+/*** BUILD CONTROL SEQUENCES BASES ON MEMORIZED POSITIONS ***/
+void LECP_Build_Control()
+{
+ bool control_seq_finished = false, position_memorized = false, delay_selected = false;
+ int selected_memorized_position = 0, ctrl_sequence = 0, pos_sequence = 0, delay_value = 0;
+ float sel_mem_pos_value = 0.0;
+
+ // add a new control sequence
+ control_sequences[0][0]++;
+
+ while(!control_seq_finished && equipment_ready) {
+
+ if(!BUT_Portocaliu) { // swap thrgough memorized positions
+ position_memorized = 0;
+ while(!position_memorized) {
+ // show actual memorized position
+ sel_mem_pos_value = memorised_positions[selected_memorized_position];
+ // clear display
+ slcd.clear();
+ // go home display
+ slcd.Home();
+ // disable dots
+ slcd.DP3(false);
+ slcd.DP2(false);
+ // print value
+ slcd.printf("%.1f", sel_mem_pos_value);
+ wait(1.0);
+
+ // select another position from positions list
+ if(!Board_But_Right && selected_memorized_position < number_of_mem_positions ) {
+ selected_memorized_position++;
+ }
+ if(!Board_But_Left && selected_memorized_position > 0 ) {
+ selected_memorized_position--;
+ }
+
+ // memorize in the control sequence the selected position from available ones
+ if(!BUT_Verde) {
+ ctrl_sequence = control_sequences[0][0];
+ control_sequences[ctrl_sequence][++pos_sequence] = sel_mem_pos_value;
+ showMessage(POSITION_RECORDED);
+ position_memorized = true;
+ }
+
+ // cancel
+ if(!BUT_Rosu) {
+ position_memorized = true;
+ }
+ }
+ wait(1.0);
+ // goto
+ }
+
+ // selectare delay
+ if(!BUT_Maro) {
+
+ delay_selected = false;
+ while(!delay_selected) {
+ // clear display & go home
+ slcd.clear();
+ slcd.Home();
+
+ // disable dots
+ slcd.DP3(false);
+ slcd.DP2(false);
+ // print value
+ slcd.printf("%d", delay_value);
+
+ if(!Board_But_Right && delay_value < 5 ) {
+ delay_value++;
+ }
+ if(!Board_But_Left && delay_value > 0 ) {
+ delay_value--;
+ }
+
+ if(!BUT_Verde) {
+ // memorize in the control sequence the selected position from available ones
+ ctrl_sequence = control_sequences[0][0];
+ control_sequences[ctrl_sequence][++pos_sequence] = 999.0; // delay special identification code
+ control_sequences[ctrl_sequence][++pos_sequence] = delay_value;
+ showMessage(POSITION_RECORDED);
+ delay_selected = true;
+ }
+
+ // cancel
+ if(!BUT_Rosu) {
+ delay_selected = true;
+ }
+ wait(1.0);
+ }
+ }
+
+ // finish control sequence
+ if(!BUT_Verde) {
+ control_seq_finished = true;
+ // save number of positions in the control sequence
+ control_sequences[ctrl_sequence][0] = pos_sequence;
+ showMessage(CTRL_CREATED);
+ }
+
+ // cancel control sequence
+ if(!BUT_Rosu) {
+ showMessage(ESC);
+ control_seq_finished = true;
+ for(int i = 0; i < 10; i++) {
+ control_sequences[ctrl_sequence][i] = 0;
+ }
+ control_sequences[0][0]--;
+ }
+ }
+}
+
+/*** Run control sequence ***/
+void LECP_Run_Control()
+{
+ // decompozitie pozitii in pozitiile momorate in controler si in delay-uri;
+
+ int operations[10] = {0}, index, i, j;
+ float decompose;
+
+ if(position != 0.0) {
+ LECP_return_to_origin();
+ }
+
+ while(BUT_Rosu) { // cat timp nu este oprita secventa de control
+ // decompozitie doar o singura data va trebui rulata
+ index = 1;
+ for( i = 1; i <= control_sequences[1][0]; i++) {
+
+ //decompose = control_sequences[1][i]-position;
+ if(i>1) {
+ decompose = control_sequences[1][i]-control_sequences[1][i-1];
+ } else decompose = control_sequences[1][i]-position;
+
+ if(decompose > 0 && decompose != 999.0 ) {
+ while(decompose) {
+ for( j = 1; positions[j] < decompose && j <= 30; j++) {
+ }
+ if(decompose > 0) decompose -= positions[j];
+ operations[index++] = j;
+ }
+
+ }
+ if(decompose < 0) {
+ while(decompose) {
+ for( j = 31; positions[j] > decompose && j <= 60; j++) {
+ }
+ if(decompose < 0) decompose = decompose + ( positions[j] * -1.0);
+ operations[index++] = j;
+ }
+ }
+ if(decompose == 999.0) {
+ wait(control_sequences[1][i++]);
+ }
+ /*
+ for(int k = 0; k < index; k++) {
+ if(previous <= control_sequences[1][i]) {
+ move_forward = operations[k]; // nu e prea bine dar va merge
+ } else {
+ move_backward = operations[k] + 30;
+ }
+ LECP_move_axis();
+ }
+ */
+ }
+ // add the absolute 0 position - pentru a nu avea offset la prima pozitie pentru dupa primul ciclu
+ operations[index++] = 0;
+ operations[0] = index;
+ LECP_Run(operations);
+ }
+}
+
+
+/*** SHOW INDEX and CORESPONDING INCREMENT WHILE JOGING***/
+void showIndex_Inc(int index)
+{
+ slcd.DP3(false);
+ slcd.DP2(false);
+ slcd.DP1(false);
+
+ slcd.clear();
+ slcd.Home();
+ slcd.printf(" %d",index); // print actual position index
+ wait(0.5);
+ slcd.clear();
+ slcd.Home();
+ slcd.printf("%.1f", positions[index]); // print position increment coresponding to the actual position index
+ wait(0.5);
+}
+
+/*** Display Axis Calculated Poistion ***/
+void axis_position()
+{
+ slcd.clear();
+ slcd.Home();
+
+ if(position < 10) {
+ slcd.CharPosition = 2;
+ }
+ if(position < 100 && position > 10) {
+ slcd.CharPosition = 1;
+ }
+ if(position >= 100) {
+ slcd.CharPosition = 0;
+ }
+
+ slcd.DP3(false);
+ slcd.DP2(false);
+ slcd.printf("%.1f", position);
+ wait(0.5);
+ if(position > 155.0 || position < 0.0 ) {
+ slcd.printf("A.L.M. .", position);
+ wait(1.0);
+ }
+}
+
+/*** MOVE COMMAND ***/
+bool LECP_move_axis()
+{
+ int move_axis_task = 0, alarm = 0;
+ bool move_axis_done = 0;
+
+ // load step data
+ if(move_forward) step_number = move_forward;
+ else step_number = move_backward;
+
+ move_forward = move_backward = 0;
+
+ showMessage(LECP6_SCAN_INPUTS);
+
+ do {
+ switch(move_axis_task) {
+ case 0:
+ // if not in fault or alarm
+ DO_A13_SVON = true; // turn servo on;
+ move_axis_task++; // next task
+ wait(0.25);
+ break;
+
+ case 1: // check if SVRE bit is on
+ if(LECP6_SVRE()) {
+ DO_A11_DRIVE = true; // set drive bit
+ showMessage(LECP6_MOVES);
+
+ DO_A11_DRIVE = false; // reset drive bit
+
+ move_axis_task++; // next task
+ } else {
+ alarm = 1;
+ move_axis_task = 100;
+ }
+
+ break;
+
+ case 2:
+ if(!LECP6_BUSY()) { // check to see if LECP6 is busy for
+ move_axis_task++; // next task
+ } else {
+ alarm = 2;
+ move_axis_task = 100;
+ }
+ break;
+
+ case 3:
+ do {
+ wait(0.25);
+ } while(DI_B10_INP);
+ DO_A13_SVON = false;
+ showMessage(LECP6_DONE);
+
+ move_axis_done = true;
+ move_axis_task++;
+ break;
+
+ default:
+ break;
+ }
+ } while(!move_axis_done || alarm);
+
+ if(alarm) {
+ LECP6_disable_outputs();
+ return 0;
+ }
+
+ position += positions[step_number]; // calculate new position
+ axis_position();
+ return 1;
+}
+
+
+/*** Configure Smart Axis ***/
+void config()
+{
+ LECP6_disable_outputs();
+
+ // configure RED LED to toglle every 1.0 second
+ operating_led.attach(&operating_flag,1.0);
+ equipment_status.attach(&LECP6_read_inputs, 0.25);
+
+ wait(2.0);
+}
+
+/*** read inputs from LCPE ***/
+void LECP6_read_inputs()
+{
+ sys_Status = equipment_system_status;
+ equip_active = equipment_active_step;
+
+ // check if there is power to CTRL unit of SMC axis
+ if(sys_Status == NO_POWER_SUPPLY) {
+ showMessage(NO_POWER_SUPPLY);
+ equipment_ready = 0;
+ }
+
+ // check for alarms
+ if(equip_in_alarm && !Board_But_Left && !Board_But_Right) { // if alarms and both FRDM-kl46z are pressed reset alarms
+ showMessage(LECP6_RST);
+ LECP_alarms_rst();
+ equip_in_alarm = LECP_alarms();
+ } else { // show alarm message and raise flag
+ if(sys_Status != NO_POWER_SUPPLY && (sys_Status & LECP6_IN_ALARM)) {
+ showMessage(LECP6_IN_ALARM);
+ equip_in_alarm = true;
+ }
+ }
+}
+
+/********************************************testate****************************************************/
+
+/*** Disable outputs from uC to LCPE ***/
+void LECP6_disable_outputs()
+{
+ DO_A9_SETUP = DO_A10_HOLD = DO_A11_DRIVE = DO_A12_RESET = DO_A13_SVON = false;
+ step_number = 0;
+}
+
+/*** 1 second operating led ***/
+void operating_flag()
+{
+ LED = !LED;
+}
+
+void showMessage(int message)
+{
+ slcd.clear();
+ slcd.Home();
+
+ if(message == POSITION_OVERFLOW) {
+ slcd.printf("oF ");
+ }
+ if(message == POSITION_UNDERFLOW) {
+ slcd.printf("uF ");
+ }
+ if(message == POSITION_RECORDED) {
+ slcd.printf("R.E.C. ");
+ }
+ if(message == ESC) {
+ slcd.printf("e.s.c. ");
+ }
+ if(message == RETURN_TO_ORIGIN) {
+ slcd.printf("o.r.i.g.");
+ }
+ if(message == NO_POWER_SUPPLY) {
+ slcd.printf("n.o.P.S");
+ }
+ if(message == LECP6_SCAN_INPUTS) {
+ slcd.printf("s.c.a.n");
+ }
+ if(message == LECP6_MOVES) {
+ slcd.printf("m.o.v.e");
+ }
+ if(message == LECP6_DONE) {
+ slcd.printf("d.o.n.e");
+ }
+ if(message == LECP6_SETUP) {
+ slcd.printf("s.e.t. ");
+ }
+ if(message == LECP6_JOG) {
+ slcd.printf("j.o.g. ");
+ }
+ if(message == BUILD_CTRL) {
+ slcd.printf("C.t.r.N");
+ }
+ if(message == CTRL_CREATED) {
+ slcd.printf("C.t.r.S");
+ }
+ if(message == CTRL_RUN) {
+ slcd.printf("r.u.n. ");
+ }
+ if(message == LECP6_IN_ALARM) {
+ slcd.printf("A.L.r. ");
+ }
+ if(message == LECP6_RST) {
+ slcd.printf("R.S.T. ");
+ }
+ wait(1.0);
+}
+
+
+/*** RETURN TO ORIGIN COMMAND ***/
+bool LECP_return_to_origin()
+{
+ int task = 0, alarm = 0;
+ bool ret_to_orig_done = false;
+
+ do {
+ switch(task) {
+ case 0:
+ while(equip_in_alarm) {
+ }
+ task++;
+ break;
+
+ case 1: // turn servo on
+
+ DO_A13_SVON = true;
+ if(LECP6_SVRE()) task++;
+ else {
+ alarm = 2;
+ task = 99;
+ }
+ break;
+
+ case 2:
+
+ DO_A9_SETUP = true; // set drive bit
+ showMessage(LECP6_SETUP);
+
+ while(DI_B9_SETON) {
+ wait(0.1);
+ }
+ while(LECP6_BUSY()) {
+ }
+
+ DO_A9_SETUP = false; // reset drive bit
+ task++;
+ break;
+
+ case 3:
+
+ DO_A13_SVON = false;
+ showMessage(LECP6_DONE);
+ ret_to_orig_done = true;
+ position = 0.0;
+ task++;
+ break;
+
+ default:
+ break;
+ }
+ } while(!ret_to_orig_done && !alarm);
+
+ if(alarm) {
+ LECP6_disable_outputs();
+ return 0; // issues were identified while return to origin procedure
+ }
+ axis_position();
+ return 1; // succesfully returned to origin
+}
+
+/*** FREE JOG WITH AXIS & POSITION RECORD***/
+void LECP_Jog()
+{
+ bool jog_finished = false;
+ int position_index = 1;
+
+ while(!jog_finished && equipment_ready && !equip_in_alarm) {
+
+ showIndex_Inc(position_index);
+ axis_position();
+
+ if(!Board_But_Right) { // move forward if actual position + position coresponding to the actual increment does result in a axis position overflow
+ if((position + positions[position_index]) <= 150) {
+ move_forward = position_index;
+ } else {
+ showMessage(POSITION_OVERFLOW);
+ }
+ }
+
+ if(!Board_But_Left) { // move backward if actual position - position coresponding to the actual increment does result in a axis position underflow
+ if((position + positions[position_index+30]) >= 0) {
+ move_backward = position_index + 30;
+ } else {
+ showMessage(POSITION_OVERFLOW);
+ }
+ }
+
+ if(!BUT_Portocaliu && position_index < 31) {
+ position_index++; // increase actual increment
+ }
+ if(!BUT_Maro && position_index > 0) {
+ position_index--; // decrease actual increment
+ }
+
+ if(!BUT_Verde) { // record axis position
+ number_of_mem_positions++;
+ memorised_positions[number_of_mem_positions] = position;
+ showMessage(POSITION_RECORDED);
+ }
+
+ if(!BUT_Rosu) { // end free axis jog
+ showMessage(ESC);
+ jog_finished = true;
+ }
+
+ if(move_forward || move_backward) { // move axis forward or backward accordingly to the coresponding actual index position
+ LECP_move_axis();
+ }
+ }
+}
+
+/*** CHECK FOR LCEP ALARMS ***/
+bool LECP_alarms()
+{
+ // check for alarms
+ if(DI_B12_ESTOP || DI_B13_ALARM) return 1;
+ else return 0;
+}
+
+/*** RESET LCEP ALARMS ***/
+void LECP_alarms_rst()
+{
+ DO_A12_RESET = true;
+ wait(0.5);
+ DO_A12_RESET = false;
+}
+
+/*** CHECK IF SVRE BIT BECAME ACTIVE AFTER SERVO ON CMD ***/
+bool LECP6_SVRE()
+{
+// check if SVRE bit is on as an action to Servo ON
+ int timeout = 0;
+ do {
+ //wait(0.1);
+ wait_ms(50);
+ timeout++;
+ if(timeout >= 10) return 0;
+ } while(DI_B11_SVRE);
+ return 1;
+}
+
+/*** CHECK IF LCPE IS BUSY PERFORMING AN OPERATION ***/
+bool LECP6_BUSY()
+{
+ int timeout = 0;
+ do {
+ wait(0.1);
+ timeout++;
+ if(timeout >= 10) return 1;
+ } while(!DI_B7_BUSY);
+ return 0;
+}
+