Mircea Murar / Mbed 2 deprecated Smart_Axis_2015_08_30_v12

SMC Axis control

Dependencies:   SLCD mbed

main.cpp@0:bf14022d00d5, 2016-04-12 (annotated)

Committer:
Mircea3M
Date:
Tue Apr 12 12:07:04 2016 +0000
Revision:
0:bf14022d00d5
SMC Axis

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Mircea3M 0:bf14022d00d5 1 #include "mbed.h"
Mircea3M 0:bf14022d00d5 2 #include "I2C.h"
Mircea3M 0:bf14022d00d5 3 #include "SLCD.h"
Mircea3M 0:bf14022d00d5 4
Mircea3M 0:bf14022d00d5 5 // v12
Mircea3M 0:bf14022d00d5 6 // partea de control merge
Mircea3M 0:bf14022d00d5 7
Mircea3M 0:bf14022d00d5 8 // intrarile de la controlerul SMC sunt inversate
Mircea3M 0:bf14022d00d5 9
Mircea3M 0:bf14022d00d5 10 #define NO_POWER_SUPPLY 0x7F
Mircea3M 0:bf14022d00d5 11 #define LECP6_IN_ALARM 0x60
Mircea3M 0:bf14022d00d5 12 #define RETURN_TO_ORIGIN 0x02
Mircea3M 0:bf14022d00d5 13 #define LECP6_SCAN_INPUTS 0x07
Mircea3M 0:bf14022d00d5 14 #define LECP6_MOVES 0x08
Mircea3M 0:bf14022d00d5 15 #define LECP6_DONE 0x09
Mircea3M 0:bf14022d00d5 16 #define LECP6_SETUP 0x0A
Mircea3M 0:bf14022d00d5 17 #define LECP6_JOG 0x15
Mircea3M 0:bf14022d00d5 18 #define POSITION_OVERFLOW 0x11
Mircea3M 0:bf14022d00d5 19 #define POSITION_UNDERFLOW 0x12
Mircea3M 0:bf14022d00d5 20 #define POSITION_RECORDED 0x30
Mircea3M 0:bf14022d00d5 21 #define ESC 0x40
Mircea3M 0:bf14022d00d5 22 #define BUILD_CTRL 0x50
Mircea3M 0:bf14022d00d5 23 #define CTRL_CREATED 0x51
Mircea3M 0:bf14022d00d5 24 #define CTRL_RUN 0x52
Mircea3M 0:bf14022d00d5 25 #define LECP6_RST 0xFF
Mircea3M 0:bf14022d00d5 26
Mircea3M 0:bf14022d00d5 27
Mircea3M 0:bf14022d00d5 28 Ticker operating_led;
Mircea3M 0:bf14022d00d5 29 Ticker equipment_status;
Mircea3M 0:bf14022d00d5 30
Mircea3M 0:bf14022d00d5 31 // 4 digit LCD
Mircea3M 0:bf14022d00d5 32 SLCD slcd;
Mircea3M 0:bf14022d00d5 33 /*
Mircea3M 0:bf14022d00d5 34 I2C I2C_Comm(PTC9,PTC8);
Mircea3M 0:bf14022d00d5 35 I2C I2C_LCD(PTC2,PTC1);
Mircea3M 0:bf14022d00d5 36 const int addr = 0x90;
Mircea3M 0:bf14022d00d5 37 //Serial UART_Comm(PTA2, PTA1);
Mircea3M 0:bf14022d00d5 38 */
Mircea3M 0:bf14022d00d5 39 DigitalOut LED(PTE29);
Mircea3M 0:bf14022d00d5 40 DigitalOut Relay_CTRL(PTE31);
Mircea3M 0:bf14022d00d5 41 DigitalOut HP_Output(PTB19);
Mircea3M 0:bf14022d00d5 42 DigitalIn Board_But_Left(PTC12);
Mircea3M 0:bf14022d00d5 43 DigitalIn Board_But_Right(PTC3);
Mircea3M 0:bf14022d00d5 44
Mircea3M 0:bf14022d00d5 45 // MCU Digital Output to control Axis/Gripper
Mircea3M 0:bf14022d00d5 46 /*
Mircea3M 0:bf14022d00d5 47 DigitalOut DO_A3_IN0(PTE18);
Mircea3M 0:bf14022d00d5 48 DigitalOut DO_A4_IN1(PTE17);
Mircea3M 0:bf14022d00d5 49 DigitalOut DO_A5_IN2(PTE16);
Mircea3M 0:bf14022d00d5 50 DigitalOut DO_A6_IN3(PTE6);
Mircea3M 0:bf14022d00d5 51 DigitalOut DO_A7_IN4(PTE3);
Mircea3M 0:bf14022d00d5 52 DigitalOut DO_A8_IN5(PTE2);
Mircea3M 0:bf14022d00d5 53 */
Mircea3M 0:bf14022d00d5 54 DigitalOut DO_A9_SETUP(PTA14);
Mircea3M 0:bf14022d00d5 55 DigitalOut DO_A10_HOLD(PTA6);
Mircea3M 0:bf14022d00d5 56 DigitalOut DO_A11_DRIVE(PTA7);
Mircea3M 0:bf14022d00d5 57 DigitalOut DO_A12_RESET(PTC16);
Mircea3M 0:bf14022d00d5 58 DigitalOut DO_A13_SVON(PTC13);
Mircea3M 0:bf14022d00d5 59
Mircea3M 0:bf14022d00d5 60 // MCU Digital Input for feedback from Axis/Gripper
Mircea3M 0:bf14022d00d5 61 DigitalIn DI_B1_OUT0(PTE19);
Mircea3M 0:bf14022d00d5 62 DigitalIn DI_B2_OUT1(PTE1);
Mircea3M 0:bf14022d00d5 63 DigitalIn DI_B3_OUT2(PTD7);
Mircea3M 0:bf14022d00d5 64 DigitalIn DI_B4_OUT3(PTA17);
Mircea3M 0:bf14022d00d5 65 DigitalIn DI_B5_OUT4(PTA16);
Mircea3M 0:bf14022d00d5 66 DigitalIn DI_B6_OUT5(PTA15);
Mircea3M 0:bf14022d00d5 67 DigitalIn DI_B7_BUSY(PTA13);
Mircea3M 0:bf14022d00d5 68 DigitalIn DI_B8_AREA(PTD2);
Mircea3M 0:bf14022d00d5 69 DigitalIn DI_B9_SETON(PTD4);
Mircea3M 0:bf14022d00d5 70 DigitalIn DI_B10_INP(PTD6);
Mircea3M 0:bf14022d00d5 71 DigitalIn DI_B11_SVRE(PTD7);
Mircea3M 0:bf14022d00d5 72 DigitalIn DI_B12_ESTOP(PTD5);
Mircea3M 0:bf14022d00d5 73 DigitalIn DI_B13_ALARM(PTE0);
Mircea3M 0:bf14022d00d5 74
Mircea3M 0:bf14022d00d5 75 BusOut step_number(PTE18,PTE17,PTE16,PTE6,PTE3,PTE2);
Mircea3M 0:bf14022d00d5 76 BusIn equipment_active_step(PTE19, PTE1, PTD7, PTA17, PTA16, PTA15);
Mircea3M 0:bf14022d00d5 77 BusIn equipment_system_status(PTA13, PTD2, PTD4, PTD6, PTD7, PTD5, PTE0);
Mircea3M 0:bf14022d00d5 78
Mircea3M 0:bf14022d00d5 79 DigitalIn OPEN_CLOSE(PTB2);
Mircea3M 0:bf14022d00d5 80
Mircea3M 0:bf14022d00d5 81 /*
Mircea3M 0:bf14022d00d5 82 I2C ioLCD(PTC1,PTC2);
Mircea3M 0:bf14022d00d5 83 DigitalIn HP_Input(PTB18);
Mircea3M 0:bf14022d00d5 84 */
Mircea3M 0:bf14022d00d5 85
Mircea3M 0:bf14022d00d5 86 DigitalIn BUT_Verde(PTE30);
Mircea3M 0:bf14022d00d5 87 DigitalIn BUT_Maro(PTB20);
Mircea3M 0:bf14022d00d5 88 DigitalIn BUT_Portocaliu(PTE23);
Mircea3M 0:bf14022d00d5 89 DigitalIn BUT_Rosu(PTB2);
Mircea3M 0:bf14022d00d5 90
Mircea3M 0:bf14022d00d5 91 /*
Mircea3M 0:bf14022d00d5 92 DigitalOut DO_1(PTA12);
Mircea3M 0:bf14022d00d5 93 DigitalOut DO_2(PTA4);
Mircea3M 0:bf14022d00d5 94 */
Mircea3M 0:bf14022d00d5 95
Mircea3M 0:bf14022d00d5 96 void config();
Mircea3M 0:bf14022d00d5 97 void operating_flag();
Mircea3M 0:bf14022d00d5 98 void LECP6_read_inputs();
Mircea3M 0:bf14022d00d5 99 void axis_position();
Mircea3M 0:bf14022d00d5 100 void showIndex_Inc(int);
Mircea3M 0:bf14022d00d5 101 void showMessage(int);
Mircea3M 0:bf14022d00d5 102
Mircea3M 0:bf14022d00d5 103 bool LECP_alarms();
Mircea3M 0:bf14022d00d5 104 void LECP_alarms_rst();
Mircea3M 0:bf14022d00d5 105 bool LECP6_SVRE();
Mircea3M 0:bf14022d00d5 106 bool LECP6_BUSY();
Mircea3M 0:bf14022d00d5 107 bool LECP_move_axis();
Mircea3M 0:bf14022d00d5 108 bool LECP_return_to_origin();
Mircea3M 0:bf14022d00d5 109 void LECP6_disable_outputs();
Mircea3M 0:bf14022d00d5 110 void LECP_Jog();
Mircea3M 0:bf14022d00d5 111 void LECP_Build_Control();
Mircea3M 0:bf14022d00d5 112 void LECP_Run_Control();
Mircea3M 0:bf14022d00d5 113 void LECP_Run(int*);
Mircea3M 0:bf14022d00d5 114
Mircea3M 0:bf14022d00d5 115
Mircea3M 0:bf14022d00d5 116 bool equipment_ready = false, equip_in_alarm = false;
Mircea3M 0:bf14022d00d5 117
Mircea3M 0:bf14022d00d5 118 int sys_Status, equip_active;
Mircea3M 0:bf14022d00d5 119 int move_forward = 0, move_backward = 0;
Mircea3M 0:bf14022d00d5 120 float memorised_positions[10];
Mircea3M 0:bf14022d00d5 121 float control_sequences[10][10];
Mircea3M 0:bf14022d00d5 122 int number_of_mem_positions = 0;
Mircea3M 0:bf14022d00d5 123 float position;
Mircea3M 0:bf14022d00d5 124 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,
Mircea3M 0:bf14022d00d5 125 -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
Mircea3M 0:bf14022d00d5 126 };
Mircea3M 0:bf14022d00d5 127
Mircea3M 0:bf14022d00d5 128 int main()
Mircea3M 0:bf14022d00d5 129 {
Mircea3M 0:bf14022d00d5 130 config();
Mircea3M 0:bf14022d00d5 131
Mircea3M 0:bf14022d00d5 132 control_sequences[1][0] = 3;
Mircea3M 0:bf14022d00d5 133 control_sequences[1][1] = 20.0;
Mircea3M 0:bf14022d00d5 134 control_sequences[1][2] = 50.0;
Mircea3M 0:bf14022d00d5 135 control_sequences[1][3] = 25.0;
Mircea3M 0:bf14022d00d5 136 /* int contr[10];
Mircea3M 0:bf14022d00d5 137 contr[0] = 4;
Mircea3M 0:bf14022d00d5 138 contr[1] = 5;
Mircea3M 0:bf14022d00d5 139 contr[2] = 28;
Mircea3M 0:bf14022d00d5 140 contr[3] = 58;
Mircea3M 0:bf14022d00d5 141 contr[4] = 35;
Mircea3M 0:bf14022d00d5 142 */
Mircea3M 0:bf14022d00d5 143 //LECP_return_to_origin();
Mircea3M 0:bf14022d00d5 144 //LECP_Run(contr);
Mircea3M 0:bf14022d00d5 145
Mircea3M 0:bf14022d00d5 146 while(1) {
Mircea3M 0:bf14022d00d5 147
Mircea3M 0:bf14022d00d5 148 /******* RETURN TO ORIGIN ********/
Mircea3M 0:bf14022d00d5 149 if(sys_Status != NO_POWER_SUPPLY) {
Mircea3M 0:bf14022d00d5 150 if(!equip_in_alarm && (!equipment_ready || !Board_But_Left)) {
Mircea3M 0:bf14022d00d5 151 showMessage(RETURN_TO_ORIGIN);
Mircea3M 0:bf14022d00d5 152 equipment_ready = LECP_return_to_origin();
Mircea3M 0:bf14022d00d5 153 }
Mircea3M 0:bf14022d00d5 154 }
Mircea3M 0:bf14022d00d5 155
Mircea3M 0:bf14022d00d5 156 /********* JOGING ***********/
Mircea3M 0:bf14022d00d5 157 if(equipment_ready && !BUT_Portocaliu) {
Mircea3M 0:bf14022d00d5 158 showMessage(LECP6_JOG);
Mircea3M 0:bf14022d00d5 159 LECP_Jog();
Mircea3M 0:bf14022d00d5 160 }
Mircea3M 0:bf14022d00d5 161
Mircea3M 0:bf14022d00d5 162 /******** CONTROL ***********/
Mircea3M 0:bf14022d00d5 163 if(equipment_ready && !BUT_Maro) {
Mircea3M 0:bf14022d00d5 164 showMessage(BUILD_CTRL);
Mircea3M 0:bf14022d00d5 165 LECP_Build_Control();
Mircea3M 0:bf14022d00d5 166 }
Mircea3M 0:bf14022d00d5 167
Mircea3M 0:bf14022d00d5 168 /******** RUN CONTROL SEQUENCE ********/
Mircea3M 0:bf14022d00d5 169 if(equipment_ready && !BUT_Verde) {
Mircea3M 0:bf14022d00d5 170 showMessage(CTRL_RUN);
Mircea3M 0:bf14022d00d5 171 LECP_Run_Control();
Mircea3M 0:bf14022d00d5 172 }
Mircea3M 0:bf14022d00d5 173 }
Mircea3M 0:bf14022d00d5 174 }
Mircea3M 0:bf14022d00d5 175
Mircea3M 0:bf14022d00d5 176
Mircea3M 0:bf14022d00d5 177
Mircea3M 0:bf14022d00d5 178 void LECP_Run(int* ctrl)
Mircea3M 0:bf14022d00d5 179 {
Mircea3M 0:bf14022d00d5 180
Mircea3M 0:bf14022d00d5 181 // in functia aceasta sa introduc si return to origin
Mircea3M 0:bf14022d00d5 182
Mircea3M 0:bf14022d00d5 183 int move_axis_task = 0, alarm = 0;
Mircea3M 0:bf14022d00d5 184 int last_step = ctrl[0], step;
Mircea3M 0:bf14022d00d5 185
Mircea3M 0:bf14022d00d5 186 while(BUT_Rosu && !equip_in_alarm && equipment_ready && !alarm) { // cat timp nu este oprita secventa de control
Mircea3M 0:bf14022d00d5 187 switch(move_axis_task) {
Mircea3M 0:bf14022d00d5 188 case 0:
Mircea3M 0:bf14022d00d5 189 // check if axis is not in fault or alarm
Mircea3M 0:bf14022d00d5 190 while(equip_in_alarm) {
Mircea3M 0:bf14022d00d5 191 }
Mircea3M 0:bf14022d00d5 192 move_axis_task++; // next task
Mircea3M 0:bf14022d00d5 193 break;
Mircea3M 0:bf14022d00d5 194
Mircea3M 0:bf14022d00d5 195 case 1:
Mircea3M 0:bf14022d00d5 196 DO_A13_SVON = true; // turn servo on;
Mircea3M 0:bf14022d00d5 197 if(LECP6_SVRE()) {
Mircea3M 0:bf14022d00d5 198 move_axis_task++; // next task
Mircea3M 0:bf14022d00d5 199 } else {
Mircea3M 0:bf14022d00d5 200 alarm = 1;
Mircea3M 0:bf14022d00d5 201 move_axis_task = 100;
Mircea3M 0:bf14022d00d5 202 }
Mircea3M 0:bf14022d00d5 203 break;
Mircea3M 0:bf14022d00d5 204
Mircea3M 0:bf14022d00d5 205 case 2:
Mircea3M 0:bf14022d00d5 206 for( step = 1; step < last_step && !equip_in_alarm; step++) {
Mircea3M 0:bf14022d00d5 207 step_number = ctrl[step];
Mircea3M 0:bf14022d00d5 208 if(step_number == 0 ) position = 0;
Mircea3M 0:bf14022d00d5 209 else position = position + positions[step_number];
Mircea3M 0:bf14022d00d5 210 DO_A11_DRIVE = true; // set drive bit
Mircea3M 0:bf14022d00d5 211 slcd.clear();
Mircea3M 0:bf14022d00d5 212 slcd.Home();
Mircea3M 0:bf14022d00d5 213 wait_ms(50);
Mircea3M 0:bf14022d00d5 214 DO_A11_DRIVE = false; // reset drive bit
Mircea3M 0:bf14022d00d5 215 slcd.printf("%.1f", position);
Mircea3M 0:bf14022d00d5 216 do {
Mircea3M 0:bf14022d00d5 217 wait_ms(10);
Mircea3M 0:bf14022d00d5 218 } while(DI_B10_INP);
Mircea3M 0:bf14022d00d5 219 do {
Mircea3M 0:bf14022d00d5 220 wait_ms(10);
Mircea3M 0:bf14022d00d5 221 } while(LECP6_BUSY());
Mircea3M 0:bf14022d00d5 222
Mircea3M 0:bf14022d00d5 223 }
Mircea3M 0:bf14022d00d5 224 if(!equip_in_alarm) move_axis_task = 0; // next task
Mircea3M 0:bf14022d00d5 225 else {
Mircea3M 0:bf14022d00d5 226 alarm = 2;
Mircea3M 0:bf14022d00d5 227 move_axis_task = 100;
Mircea3M 0:bf14022d00d5 228 }
Mircea3M 0:bf14022d00d5 229 break;
Mircea3M 0:bf14022d00d5 230
Mircea3M 0:bf14022d00d5 231 default:
Mircea3M 0:bf14022d00d5 232 break;
Mircea3M 0:bf14022d00d5 233 }
Mircea3M 0:bf14022d00d5 234 }
Mircea3M 0:bf14022d00d5 235 DO_A13_SVON = false; // turn servo off;
Mircea3M 0:bf14022d00d5 236
Mircea3M 0:bf14022d00d5 237 }
Mircea3M 0:bf14022d00d5 238
Mircea3M 0:bf14022d00d5 239 /*** BUILD CONTROL SEQUENCES BASES ON MEMORIZED POSITIONS ***/
Mircea3M 0:bf14022d00d5 240 void LECP_Build_Control()
Mircea3M 0:bf14022d00d5 241 {
Mircea3M 0:bf14022d00d5 242 bool control_seq_finished = false, position_memorized = false, delay_selected = false;
Mircea3M 0:bf14022d00d5 243 int selected_memorized_position = 0, ctrl_sequence = 0, pos_sequence = 0, delay_value = 0;
Mircea3M 0:bf14022d00d5 244 float sel_mem_pos_value = 0.0;
Mircea3M 0:bf14022d00d5 245
Mircea3M 0:bf14022d00d5 246 // add a new control sequence
Mircea3M 0:bf14022d00d5 247 control_sequences[0][0]++;
Mircea3M 0:bf14022d00d5 248
Mircea3M 0:bf14022d00d5 249 while(!control_seq_finished && equipment_ready) {
Mircea3M 0:bf14022d00d5 250
Mircea3M 0:bf14022d00d5 251 if(!BUT_Portocaliu) { // swap thrgough memorized positions
Mircea3M 0:bf14022d00d5 252 position_memorized = 0;
Mircea3M 0:bf14022d00d5 253 while(!position_memorized) {
Mircea3M 0:bf14022d00d5 254 // show actual memorized position
Mircea3M 0:bf14022d00d5 255 sel_mem_pos_value = memorised_positions[selected_memorized_position];
Mircea3M 0:bf14022d00d5 256 // clear display
Mircea3M 0:bf14022d00d5 257 slcd.clear();
Mircea3M 0:bf14022d00d5 258 // go home display
Mircea3M 0:bf14022d00d5 259 slcd.Home();
Mircea3M 0:bf14022d00d5 260 // disable dots
Mircea3M 0:bf14022d00d5 261 slcd.DP3(false);
Mircea3M 0:bf14022d00d5 262 slcd.DP2(false);
Mircea3M 0:bf14022d00d5 263 // print value
Mircea3M 0:bf14022d00d5 264 slcd.printf("%.1f", sel_mem_pos_value);
Mircea3M 0:bf14022d00d5 265 wait(1.0);
Mircea3M 0:bf14022d00d5 266
Mircea3M 0:bf14022d00d5 267 // select another position from positions list
Mircea3M 0:bf14022d00d5 268 if(!Board_But_Right && selected_memorized_position < number_of_mem_positions ) {
Mircea3M 0:bf14022d00d5 269 selected_memorized_position++;
Mircea3M 0:bf14022d00d5 270 }
Mircea3M 0:bf14022d00d5 271 if(!Board_But_Left && selected_memorized_position > 0 ) {
Mircea3M 0:bf14022d00d5 272 selected_memorized_position--;
Mircea3M 0:bf14022d00d5 273 }
Mircea3M 0:bf14022d00d5 274
Mircea3M 0:bf14022d00d5 275 // memorize in the control sequence the selected position from available ones
Mircea3M 0:bf14022d00d5 276 if(!BUT_Verde) {
Mircea3M 0:bf14022d00d5 277 ctrl_sequence = control_sequences[0][0];
Mircea3M 0:bf14022d00d5 278 control_sequences[ctrl_sequence][++pos_sequence] = sel_mem_pos_value;
Mircea3M 0:bf14022d00d5 279 showMessage(POSITION_RECORDED);
Mircea3M 0:bf14022d00d5 280 position_memorized = true;
Mircea3M 0:bf14022d00d5 281 }
Mircea3M 0:bf14022d00d5 282
Mircea3M 0:bf14022d00d5 283 // cancel
Mircea3M 0:bf14022d00d5 284 if(!BUT_Rosu) {
Mircea3M 0:bf14022d00d5 285 position_memorized = true;
Mircea3M 0:bf14022d00d5 286 }
Mircea3M 0:bf14022d00d5 287 }
Mircea3M 0:bf14022d00d5 288 wait(1.0);
Mircea3M 0:bf14022d00d5 289 // goto
Mircea3M 0:bf14022d00d5 290 }
Mircea3M 0:bf14022d00d5 291
Mircea3M 0:bf14022d00d5 292 // selectare delay
Mircea3M 0:bf14022d00d5 293 if(!BUT_Maro) {
Mircea3M 0:bf14022d00d5 294
Mircea3M 0:bf14022d00d5 295 delay_selected = false;
Mircea3M 0:bf14022d00d5 296 while(!delay_selected) {
Mircea3M 0:bf14022d00d5 297 // clear display & go home
Mircea3M 0:bf14022d00d5 298 slcd.clear();
Mircea3M 0:bf14022d00d5 299 slcd.Home();
Mircea3M 0:bf14022d00d5 300
Mircea3M 0:bf14022d00d5 301 // disable dots
Mircea3M 0:bf14022d00d5 302 slcd.DP3(false);
Mircea3M 0:bf14022d00d5 303 slcd.DP2(false);
Mircea3M 0:bf14022d00d5 304 // print value
Mircea3M 0:bf14022d00d5 305 slcd.printf("%d", delay_value);
Mircea3M 0:bf14022d00d5 306
Mircea3M 0:bf14022d00d5 307 if(!Board_But_Right && delay_value < 5 ) {
Mircea3M 0:bf14022d00d5 308 delay_value++;
Mircea3M 0:bf14022d00d5 309 }
Mircea3M 0:bf14022d00d5 310 if(!Board_But_Left && delay_value > 0 ) {
Mircea3M 0:bf14022d00d5 311 delay_value--;
Mircea3M 0:bf14022d00d5 312 }
Mircea3M 0:bf14022d00d5 313
Mircea3M 0:bf14022d00d5 314 if(!BUT_Verde) {
Mircea3M 0:bf14022d00d5 315 // memorize in the control sequence the selected position from available ones
Mircea3M 0:bf14022d00d5 316 ctrl_sequence = control_sequences[0][0];
Mircea3M 0:bf14022d00d5 317 control_sequences[ctrl_sequence][++pos_sequence] = 999.0; // delay special identification code
Mircea3M 0:bf14022d00d5 318 control_sequences[ctrl_sequence][++pos_sequence] = delay_value;
Mircea3M 0:bf14022d00d5 319 showMessage(POSITION_RECORDED);
Mircea3M 0:bf14022d00d5 320 delay_selected = true;
Mircea3M 0:bf14022d00d5 321 }
Mircea3M 0:bf14022d00d5 322
Mircea3M 0:bf14022d00d5 323 // cancel
Mircea3M 0:bf14022d00d5 324 if(!BUT_Rosu) {
Mircea3M 0:bf14022d00d5 325 delay_selected = true;
Mircea3M 0:bf14022d00d5 326 }
Mircea3M 0:bf14022d00d5 327 wait(1.0);
Mircea3M 0:bf14022d00d5 328 }
Mircea3M 0:bf14022d00d5 329 }
Mircea3M 0:bf14022d00d5 330
Mircea3M 0:bf14022d00d5 331 // finish control sequence
Mircea3M 0:bf14022d00d5 332 if(!BUT_Verde) {
Mircea3M 0:bf14022d00d5 333 control_seq_finished = true;
Mircea3M 0:bf14022d00d5 334 // save number of positions in the control sequence
Mircea3M 0:bf14022d00d5 335 control_sequences[ctrl_sequence][0] = pos_sequence;
Mircea3M 0:bf14022d00d5 336 showMessage(CTRL_CREATED);
Mircea3M 0:bf14022d00d5 337 }
Mircea3M 0:bf14022d00d5 338
Mircea3M 0:bf14022d00d5 339 // cancel control sequence
Mircea3M 0:bf14022d00d5 340 if(!BUT_Rosu) {
Mircea3M 0:bf14022d00d5 341 showMessage(ESC);
Mircea3M 0:bf14022d00d5 342 control_seq_finished = true;
Mircea3M 0:bf14022d00d5 343 for(int i = 0; i < 10; i++) {
Mircea3M 0:bf14022d00d5 344 control_sequences[ctrl_sequence][i] = 0;
Mircea3M 0:bf14022d00d5 345 }
Mircea3M 0:bf14022d00d5 346 control_sequences[0][0]--;
Mircea3M 0:bf14022d00d5 347 }
Mircea3M 0:bf14022d00d5 348 }
Mircea3M 0:bf14022d00d5 349 }
Mircea3M 0:bf14022d00d5 350
Mircea3M 0:bf14022d00d5 351 /*** Run control sequence ***/
Mircea3M 0:bf14022d00d5 352 void LECP_Run_Control()
Mircea3M 0:bf14022d00d5 353 {
Mircea3M 0:bf14022d00d5 354 // decompozitie pozitii in pozitiile momorate in controler si in delay-uri;
Mircea3M 0:bf14022d00d5 355
Mircea3M 0:bf14022d00d5 356 int operations[10] = {0}, index, i, j;
Mircea3M 0:bf14022d00d5 357 float decompose;
Mircea3M 0:bf14022d00d5 358
Mircea3M 0:bf14022d00d5 359 if(position != 0.0) {
Mircea3M 0:bf14022d00d5 360 LECP_return_to_origin();
Mircea3M 0:bf14022d00d5 361 }
Mircea3M 0:bf14022d00d5 362
Mircea3M 0:bf14022d00d5 363 while(BUT_Rosu) { // cat timp nu este oprita secventa de control
Mircea3M 0:bf14022d00d5 364 // decompozitie doar o singura data va trebui rulata
Mircea3M 0:bf14022d00d5 365 index = 1;
Mircea3M 0:bf14022d00d5 366 for( i = 1; i <= control_sequences[1][0]; i++) {
Mircea3M 0:bf14022d00d5 367
Mircea3M 0:bf14022d00d5 368 //decompose = control_sequences[1][i]-position;
Mircea3M 0:bf14022d00d5 369 if(i>1) {
Mircea3M 0:bf14022d00d5 370 decompose = control_sequences[1][i]-control_sequences[1][i-1];
Mircea3M 0:bf14022d00d5 371 } else decompose = control_sequences[1][i]-position;
Mircea3M 0:bf14022d00d5 372
Mircea3M 0:bf14022d00d5 373 if(decompose > 0 && decompose != 999.0 ) {
Mircea3M 0:bf14022d00d5 374 while(decompose) {
Mircea3M 0:bf14022d00d5 375 for( j = 1; positions[j] < decompose && j <= 30; j++) {
Mircea3M 0:bf14022d00d5 376 }
Mircea3M 0:bf14022d00d5 377 if(decompose > 0) decompose -= positions[j];
Mircea3M 0:bf14022d00d5 378 operations[index++] = j;
Mircea3M 0:bf14022d00d5 379 }
Mircea3M 0:bf14022d00d5 380
Mircea3M 0:bf14022d00d5 381 }
Mircea3M 0:bf14022d00d5 382 if(decompose < 0) {
Mircea3M 0:bf14022d00d5 383 while(decompose) {
Mircea3M 0:bf14022d00d5 384 for( j = 31; positions[j] > decompose && j <= 60; j++) {
Mircea3M 0:bf14022d00d5 385 }
Mircea3M 0:bf14022d00d5 386 if(decompose < 0) decompose = decompose + ( positions[j] * -1.0);
Mircea3M 0:bf14022d00d5 387 operations[index++] = j;
Mircea3M 0:bf14022d00d5 388 }
Mircea3M 0:bf14022d00d5 389 }
Mircea3M 0:bf14022d00d5 390 if(decompose == 999.0) {
Mircea3M 0:bf14022d00d5 391 wait(control_sequences[1][i++]);
Mircea3M 0:bf14022d00d5 392 }
Mircea3M 0:bf14022d00d5 393 /*
Mircea3M 0:bf14022d00d5 394 for(int k = 0; k < index; k++) {
Mircea3M 0:bf14022d00d5 395 if(previous <= control_sequences[1][i]) {
Mircea3M 0:bf14022d00d5 396 move_forward = operations[k]; // nu e prea bine dar va merge
Mircea3M 0:bf14022d00d5 397 } else {
Mircea3M 0:bf14022d00d5 398 move_backward = operations[k] + 30;
Mircea3M 0:bf14022d00d5 399 }
Mircea3M 0:bf14022d00d5 400 LECP_move_axis();
Mircea3M 0:bf14022d00d5 401 }
Mircea3M 0:bf14022d00d5 402 */
Mircea3M 0:bf14022d00d5 403 }
Mircea3M 0:bf14022d00d5 404 // add the absolute 0 position - pentru a nu avea offset la prima pozitie pentru dupa primul ciclu
Mircea3M 0:bf14022d00d5 405 operations[index++] = 0;
Mircea3M 0:bf14022d00d5 406 operations[0] = index;
Mircea3M 0:bf14022d00d5 407 LECP_Run(operations);
Mircea3M 0:bf14022d00d5 408 }
Mircea3M 0:bf14022d00d5 409 }
Mircea3M 0:bf14022d00d5 410
Mircea3M 0:bf14022d00d5 411
Mircea3M 0:bf14022d00d5 412 /*** SHOW INDEX and CORESPONDING INCREMENT WHILE JOGING***/
Mircea3M 0:bf14022d00d5 413 void showIndex_Inc(int index)
Mircea3M 0:bf14022d00d5 414 {
Mircea3M 0:bf14022d00d5 415 slcd.DP3(false);
Mircea3M 0:bf14022d00d5 416 slcd.DP2(false);
Mircea3M 0:bf14022d00d5 417 slcd.DP1(false);
Mircea3M 0:bf14022d00d5 418
Mircea3M 0:bf14022d00d5 419 slcd.clear();
Mircea3M 0:bf14022d00d5 420 slcd.Home();
Mircea3M 0:bf14022d00d5 421 slcd.printf(" %d",index); // print actual position index
Mircea3M 0:bf14022d00d5 422 wait(0.5);
Mircea3M 0:bf14022d00d5 423 slcd.clear();
Mircea3M 0:bf14022d00d5 424 slcd.Home();
Mircea3M 0:bf14022d00d5 425 slcd.printf("%.1f", positions[index]); // print position increment coresponding to the actual position index
Mircea3M 0:bf14022d00d5 426 wait(0.5);
Mircea3M 0:bf14022d00d5 427 }
Mircea3M 0:bf14022d00d5 428
Mircea3M 0:bf14022d00d5 429 /*** Display Axis Calculated Poistion ***/
Mircea3M 0:bf14022d00d5 430 void axis_position()
Mircea3M 0:bf14022d00d5 431 {
Mircea3M 0:bf14022d00d5 432 slcd.clear();
Mircea3M 0:bf14022d00d5 433 slcd.Home();
Mircea3M 0:bf14022d00d5 434
Mircea3M 0:bf14022d00d5 435 if(position < 10) {
Mircea3M 0:bf14022d00d5 436 slcd.CharPosition = 2;
Mircea3M 0:bf14022d00d5 437 }
Mircea3M 0:bf14022d00d5 438 if(position < 100 && position > 10) {
Mircea3M 0:bf14022d00d5 439 slcd.CharPosition = 1;
Mircea3M 0:bf14022d00d5 440 }
Mircea3M 0:bf14022d00d5 441 if(position >= 100) {
Mircea3M 0:bf14022d00d5 442 slcd.CharPosition = 0;
Mircea3M 0:bf14022d00d5 443 }
Mircea3M 0:bf14022d00d5 444
Mircea3M 0:bf14022d00d5 445 slcd.DP3(false);
Mircea3M 0:bf14022d00d5 446 slcd.DP2(false);
Mircea3M 0:bf14022d00d5 447 slcd.printf("%.1f", position);
Mircea3M 0:bf14022d00d5 448 wait(0.5);
Mircea3M 0:bf14022d00d5 449 if(position > 155.0 || position < 0.0 ) {
Mircea3M 0:bf14022d00d5 450 slcd.printf("A.L.M. .", position);
Mircea3M 0:bf14022d00d5 451 wait(1.0);
Mircea3M 0:bf14022d00d5 452 }
Mircea3M 0:bf14022d00d5 453 }
Mircea3M 0:bf14022d00d5 454
Mircea3M 0:bf14022d00d5 455 /*** MOVE COMMAND ***/
Mircea3M 0:bf14022d00d5 456 bool LECP_move_axis()
Mircea3M 0:bf14022d00d5 457 {
Mircea3M 0:bf14022d00d5 458 int move_axis_task = 0, alarm = 0;
Mircea3M 0:bf14022d00d5 459 bool move_axis_done = 0;
Mircea3M 0:bf14022d00d5 460
Mircea3M 0:bf14022d00d5 461 // load step data
Mircea3M 0:bf14022d00d5 462 if(move_forward) step_number = move_forward;
Mircea3M 0:bf14022d00d5 463 else step_number = move_backward;
Mircea3M 0:bf14022d00d5 464
Mircea3M 0:bf14022d00d5 465 move_forward = move_backward = 0;
Mircea3M 0:bf14022d00d5 466
Mircea3M 0:bf14022d00d5 467 showMessage(LECP6_SCAN_INPUTS);
Mircea3M 0:bf14022d00d5 468
Mircea3M 0:bf14022d00d5 469 do {
Mircea3M 0:bf14022d00d5 470 switch(move_axis_task) {
Mircea3M 0:bf14022d00d5 471 case 0:
Mircea3M 0:bf14022d00d5 472 // if not in fault or alarm
Mircea3M 0:bf14022d00d5 473 DO_A13_SVON = true; // turn servo on;
Mircea3M 0:bf14022d00d5 474 move_axis_task++; // next task
Mircea3M 0:bf14022d00d5 475 wait(0.25);
Mircea3M 0:bf14022d00d5 476 break;
Mircea3M 0:bf14022d00d5 477
Mircea3M 0:bf14022d00d5 478 case 1: // check if SVRE bit is on
Mircea3M 0:bf14022d00d5 479 if(LECP6_SVRE()) {
Mircea3M 0:bf14022d00d5 480 DO_A11_DRIVE = true; // set drive bit
Mircea3M 0:bf14022d00d5 481 showMessage(LECP6_MOVES);
Mircea3M 0:bf14022d00d5 482
Mircea3M 0:bf14022d00d5 483 DO_A11_DRIVE = false; // reset drive bit
Mircea3M 0:bf14022d00d5 484
Mircea3M 0:bf14022d00d5 485 move_axis_task++; // next task
Mircea3M 0:bf14022d00d5 486 } else {
Mircea3M 0:bf14022d00d5 487 alarm = 1;
Mircea3M 0:bf14022d00d5 488 move_axis_task = 100;
Mircea3M 0:bf14022d00d5 489 }
Mircea3M 0:bf14022d00d5 490
Mircea3M 0:bf14022d00d5 491 break;
Mircea3M 0:bf14022d00d5 492
Mircea3M 0:bf14022d00d5 493 case 2:
Mircea3M 0:bf14022d00d5 494 if(!LECP6_BUSY()) { // check to see if LECP6 is busy for
Mircea3M 0:bf14022d00d5 495 move_axis_task++; // next task
Mircea3M 0:bf14022d00d5 496 } else {
Mircea3M 0:bf14022d00d5 497 alarm = 2;
Mircea3M 0:bf14022d00d5 498 move_axis_task = 100;
Mircea3M 0:bf14022d00d5 499 }
Mircea3M 0:bf14022d00d5 500 break;
Mircea3M 0:bf14022d00d5 501
Mircea3M 0:bf14022d00d5 502 case 3:
Mircea3M 0:bf14022d00d5 503 do {
Mircea3M 0:bf14022d00d5 504 wait(0.25);
Mircea3M 0:bf14022d00d5 505 } while(DI_B10_INP);
Mircea3M 0:bf14022d00d5 506 DO_A13_SVON = false;
Mircea3M 0:bf14022d00d5 507 showMessage(LECP6_DONE);
Mircea3M 0:bf14022d00d5 508
Mircea3M 0:bf14022d00d5 509 move_axis_done = true;
Mircea3M 0:bf14022d00d5 510 move_axis_task++;
Mircea3M 0:bf14022d00d5 511 break;
Mircea3M 0:bf14022d00d5 512
Mircea3M 0:bf14022d00d5 513 default:
Mircea3M 0:bf14022d00d5 514 break;
Mircea3M 0:bf14022d00d5 515 }
Mircea3M 0:bf14022d00d5 516 } while(!move_axis_done || alarm);
Mircea3M 0:bf14022d00d5 517
Mircea3M 0:bf14022d00d5 518 if(alarm) {
Mircea3M 0:bf14022d00d5 519 LECP6_disable_outputs();
Mircea3M 0:bf14022d00d5 520 return 0;
Mircea3M 0:bf14022d00d5 521 }
Mircea3M 0:bf14022d00d5 522
Mircea3M 0:bf14022d00d5 523 position += positions[step_number]; // calculate new position
Mircea3M 0:bf14022d00d5 524 axis_position();
Mircea3M 0:bf14022d00d5 525 return 1;
Mircea3M 0:bf14022d00d5 526 }
Mircea3M 0:bf14022d00d5 527
Mircea3M 0:bf14022d00d5 528
Mircea3M 0:bf14022d00d5 529 /*** Configure Smart Axis ***/
Mircea3M 0:bf14022d00d5 530 void config()
Mircea3M 0:bf14022d00d5 531 {
Mircea3M 0:bf14022d00d5 532 LECP6_disable_outputs();
Mircea3M 0:bf14022d00d5 533
Mircea3M 0:bf14022d00d5 534 // configure RED LED to toglle every 1.0 second
Mircea3M 0:bf14022d00d5 535 operating_led.attach(&operating_flag,1.0);
Mircea3M 0:bf14022d00d5 536 equipment_status.attach(&LECP6_read_inputs, 0.25);
Mircea3M 0:bf14022d00d5 537
Mircea3M 0:bf14022d00d5 538 wait(2.0);
Mircea3M 0:bf14022d00d5 539 }
Mircea3M 0:bf14022d00d5 540
Mircea3M 0:bf14022d00d5 541 /*** read inputs from LCPE ***/
Mircea3M 0:bf14022d00d5 542 void LECP6_read_inputs()
Mircea3M 0:bf14022d00d5 543 {
Mircea3M 0:bf14022d00d5 544 sys_Status = equipment_system_status;
Mircea3M 0:bf14022d00d5 545 equip_active = equipment_active_step;
Mircea3M 0:bf14022d00d5 546
Mircea3M 0:bf14022d00d5 547 // check if there is power to CTRL unit of SMC axis
Mircea3M 0:bf14022d00d5 548 if(sys_Status == NO_POWER_SUPPLY) {
Mircea3M 0:bf14022d00d5 549 showMessage(NO_POWER_SUPPLY);
Mircea3M 0:bf14022d00d5 550 equipment_ready = 0;
Mircea3M 0:bf14022d00d5 551 }
Mircea3M 0:bf14022d00d5 552
Mircea3M 0:bf14022d00d5 553 // check for alarms
Mircea3M 0:bf14022d00d5 554 if(equip_in_alarm && !Board_But_Left && !Board_But_Right) { // if alarms and both FRDM-kl46z are pressed reset alarms
Mircea3M 0:bf14022d00d5 555 showMessage(LECP6_RST);
Mircea3M 0:bf14022d00d5 556 LECP_alarms_rst();
Mircea3M 0:bf14022d00d5 557 equip_in_alarm = LECP_alarms();
Mircea3M 0:bf14022d00d5 558 } else { // show alarm message and raise flag
Mircea3M 0:bf14022d00d5 559 if(sys_Status != NO_POWER_SUPPLY && (sys_Status & LECP6_IN_ALARM)) {
Mircea3M 0:bf14022d00d5 560 showMessage(LECP6_IN_ALARM);
Mircea3M 0:bf14022d00d5 561 equip_in_alarm = true;
Mircea3M 0:bf14022d00d5 562 }
Mircea3M 0:bf14022d00d5 563 }
Mircea3M 0:bf14022d00d5 564 }
Mircea3M 0:bf14022d00d5 565
Mircea3M 0:bf14022d00d5 566 /********************************************testate****************************************************/
Mircea3M 0:bf14022d00d5 567
Mircea3M 0:bf14022d00d5 568 /*** Disable outputs from uC to LCPE ***/
Mircea3M 0:bf14022d00d5 569 void LECP6_disable_outputs()
Mircea3M 0:bf14022d00d5 570 {
Mircea3M 0:bf14022d00d5 571 DO_A9_SETUP = DO_A10_HOLD = DO_A11_DRIVE = DO_A12_RESET = DO_A13_SVON = false;
Mircea3M 0:bf14022d00d5 572 step_number = 0;
Mircea3M 0:bf14022d00d5 573 }
Mircea3M 0:bf14022d00d5 574
Mircea3M 0:bf14022d00d5 575 /*** 1 second operating led ***/
Mircea3M 0:bf14022d00d5 576 void operating_flag()
Mircea3M 0:bf14022d00d5 577 {
Mircea3M 0:bf14022d00d5 578 LED = !LED;
Mircea3M 0:bf14022d00d5 579 }
Mircea3M 0:bf14022d00d5 580
Mircea3M 0:bf14022d00d5 581 void showMessage(int message)
Mircea3M 0:bf14022d00d5 582 {
Mircea3M 0:bf14022d00d5 583 slcd.clear();
Mircea3M 0:bf14022d00d5 584 slcd.Home();
Mircea3M 0:bf14022d00d5 585
Mircea3M 0:bf14022d00d5 586 if(message == POSITION_OVERFLOW) {
Mircea3M 0:bf14022d00d5 587 slcd.printf("oF ");
Mircea3M 0:bf14022d00d5 588 }
Mircea3M 0:bf14022d00d5 589 if(message == POSITION_UNDERFLOW) {
Mircea3M 0:bf14022d00d5 590 slcd.printf("uF ");
Mircea3M 0:bf14022d00d5 591 }
Mircea3M 0:bf14022d00d5 592 if(message == POSITION_RECORDED) {
Mircea3M 0:bf14022d00d5 593 slcd.printf("R.E.C. ");
Mircea3M 0:bf14022d00d5 594 }
Mircea3M 0:bf14022d00d5 595 if(message == ESC) {
Mircea3M 0:bf14022d00d5 596 slcd.printf("e.s.c. ");
Mircea3M 0:bf14022d00d5 597 }
Mircea3M 0:bf14022d00d5 598 if(message == RETURN_TO_ORIGIN) {
Mircea3M 0:bf14022d00d5 599 slcd.printf("o.r.i.g.");
Mircea3M 0:bf14022d00d5 600 }
Mircea3M 0:bf14022d00d5 601 if(message == NO_POWER_SUPPLY) {
Mircea3M 0:bf14022d00d5 602 slcd.printf("n.o.P.S");
Mircea3M 0:bf14022d00d5 603 }
Mircea3M 0:bf14022d00d5 604 if(message == LECP6_SCAN_INPUTS) {
Mircea3M 0:bf14022d00d5 605 slcd.printf("s.c.a.n");
Mircea3M 0:bf14022d00d5 606 }
Mircea3M 0:bf14022d00d5 607 if(message == LECP6_MOVES) {
Mircea3M 0:bf14022d00d5 608 slcd.printf("m.o.v.e");
Mircea3M 0:bf14022d00d5 609 }
Mircea3M 0:bf14022d00d5 610 if(message == LECP6_DONE) {
Mircea3M 0:bf14022d00d5 611 slcd.printf("d.o.n.e");
Mircea3M 0:bf14022d00d5 612 }
Mircea3M 0:bf14022d00d5 613 if(message == LECP6_SETUP) {
Mircea3M 0:bf14022d00d5 614 slcd.printf("s.e.t. ");
Mircea3M 0:bf14022d00d5 615 }
Mircea3M 0:bf14022d00d5 616 if(message == LECP6_JOG) {
Mircea3M 0:bf14022d00d5 617 slcd.printf("j.o.g. ");
Mircea3M 0:bf14022d00d5 618 }
Mircea3M 0:bf14022d00d5 619 if(message == BUILD_CTRL) {
Mircea3M 0:bf14022d00d5 620 slcd.printf("C.t.r.N");
Mircea3M 0:bf14022d00d5 621 }
Mircea3M 0:bf14022d00d5 622 if(message == CTRL_CREATED) {
Mircea3M 0:bf14022d00d5 623 slcd.printf("C.t.r.S");
Mircea3M 0:bf14022d00d5 624 }
Mircea3M 0:bf14022d00d5 625 if(message == CTRL_RUN) {
Mircea3M 0:bf14022d00d5 626 slcd.printf("r.u.n. ");
Mircea3M 0:bf14022d00d5 627 }
Mircea3M 0:bf14022d00d5 628 if(message == LECP6_IN_ALARM) {
Mircea3M 0:bf14022d00d5 629 slcd.printf("A.L.r. ");
Mircea3M 0:bf14022d00d5 630 }
Mircea3M 0:bf14022d00d5 631 if(message == LECP6_RST) {
Mircea3M 0:bf14022d00d5 632 slcd.printf("R.S.T. ");
Mircea3M 0:bf14022d00d5 633 }
Mircea3M 0:bf14022d00d5 634 wait(1.0);
Mircea3M 0:bf14022d00d5 635 }
Mircea3M 0:bf14022d00d5 636
Mircea3M 0:bf14022d00d5 637
Mircea3M 0:bf14022d00d5 638 /*** RETURN TO ORIGIN COMMAND ***/
Mircea3M 0:bf14022d00d5 639 bool LECP_return_to_origin()
Mircea3M 0:bf14022d00d5 640 {
Mircea3M 0:bf14022d00d5 641 int task = 0, alarm = 0;
Mircea3M 0:bf14022d00d5 642 bool ret_to_orig_done = false;
Mircea3M 0:bf14022d00d5 643
Mircea3M 0:bf14022d00d5 644 do {
Mircea3M 0:bf14022d00d5 645 switch(task) {
Mircea3M 0:bf14022d00d5 646 case 0:
Mircea3M 0:bf14022d00d5 647 while(equip_in_alarm) {
Mircea3M 0:bf14022d00d5 648 }
Mircea3M 0:bf14022d00d5 649 task++;
Mircea3M 0:bf14022d00d5 650 break;
Mircea3M 0:bf14022d00d5 651
Mircea3M 0:bf14022d00d5 652 case 1: // turn servo on
Mircea3M 0:bf14022d00d5 653
Mircea3M 0:bf14022d00d5 654 DO_A13_SVON = true;
Mircea3M 0:bf14022d00d5 655 if(LECP6_SVRE()) task++;
Mircea3M 0:bf14022d00d5 656 else {
Mircea3M 0:bf14022d00d5 657 alarm = 2;
Mircea3M 0:bf14022d00d5 658 task = 99;
Mircea3M 0:bf14022d00d5 659 }
Mircea3M 0:bf14022d00d5 660 break;
Mircea3M 0:bf14022d00d5 661
Mircea3M 0:bf14022d00d5 662 case 2:
Mircea3M 0:bf14022d00d5 663
Mircea3M 0:bf14022d00d5 664 DO_A9_SETUP = true; // set drive bit
Mircea3M 0:bf14022d00d5 665 showMessage(LECP6_SETUP);
Mircea3M 0:bf14022d00d5 666
Mircea3M 0:bf14022d00d5 667 while(DI_B9_SETON) {
Mircea3M 0:bf14022d00d5 668 wait(0.1);
Mircea3M 0:bf14022d00d5 669 }
Mircea3M 0:bf14022d00d5 670 while(LECP6_BUSY()) {
Mircea3M 0:bf14022d00d5 671 }
Mircea3M 0:bf14022d00d5 672
Mircea3M 0:bf14022d00d5 673 DO_A9_SETUP = false; // reset drive bit
Mircea3M 0:bf14022d00d5 674 task++;
Mircea3M 0:bf14022d00d5 675 break;
Mircea3M 0:bf14022d00d5 676
Mircea3M 0:bf14022d00d5 677 case 3:
Mircea3M 0:bf14022d00d5 678
Mircea3M 0:bf14022d00d5 679 DO_A13_SVON = false;
Mircea3M 0:bf14022d00d5 680 showMessage(LECP6_DONE);
Mircea3M 0:bf14022d00d5 681 ret_to_orig_done = true;
Mircea3M 0:bf14022d00d5 682 position = 0.0;
Mircea3M 0:bf14022d00d5 683 task++;
Mircea3M 0:bf14022d00d5 684 break;
Mircea3M 0:bf14022d00d5 685
Mircea3M 0:bf14022d00d5 686 default:
Mircea3M 0:bf14022d00d5 687 break;
Mircea3M 0:bf14022d00d5 688 }
Mircea3M 0:bf14022d00d5 689 } while(!ret_to_orig_done && !alarm);
Mircea3M 0:bf14022d00d5 690
Mircea3M 0:bf14022d00d5 691 if(alarm) {
Mircea3M 0:bf14022d00d5 692 LECP6_disable_outputs();
Mircea3M 0:bf14022d00d5 693 return 0; // issues were identified while return to origin procedure
Mircea3M 0:bf14022d00d5 694 }
Mircea3M 0:bf14022d00d5 695 axis_position();
Mircea3M 0:bf14022d00d5 696 return 1; // succesfully returned to origin
Mircea3M 0:bf14022d00d5 697 }
Mircea3M 0:bf14022d00d5 698
Mircea3M 0:bf14022d00d5 699 /*** FREE JOG WITH AXIS & POSITION RECORD***/
Mircea3M 0:bf14022d00d5 700 void LECP_Jog()
Mircea3M 0:bf14022d00d5 701 {
Mircea3M 0:bf14022d00d5 702 bool jog_finished = false;
Mircea3M 0:bf14022d00d5 703 int position_index = 1;
Mircea3M 0:bf14022d00d5 704
Mircea3M 0:bf14022d00d5 705 while(!jog_finished && equipment_ready && !equip_in_alarm) {
Mircea3M 0:bf14022d00d5 706
Mircea3M 0:bf14022d00d5 707 showIndex_Inc(position_index);
Mircea3M 0:bf14022d00d5 708 axis_position();
Mircea3M 0:bf14022d00d5 709
Mircea3M 0:bf14022d00d5 710 if(!Board_But_Right) { // move forward if actual position + position coresponding to the actual increment does result in a axis position overflow
Mircea3M 0:bf14022d00d5 711 if((position + positions[position_index]) <= 150) {
Mircea3M 0:bf14022d00d5 712 move_forward = position_index;
Mircea3M 0:bf14022d00d5 713 } else {
Mircea3M 0:bf14022d00d5 714 showMessage(POSITION_OVERFLOW);
Mircea3M 0:bf14022d00d5 715 }
Mircea3M 0:bf14022d00d5 716 }
Mircea3M 0:bf14022d00d5 717
Mircea3M 0:bf14022d00d5 718 if(!Board_But_Left) { // move backward if actual position - position coresponding to the actual increment does result in a axis position underflow
Mircea3M 0:bf14022d00d5 719 if((position + positions[position_index+30]) >= 0) {
Mircea3M 0:bf14022d00d5 720 move_backward = position_index + 30;
Mircea3M 0:bf14022d00d5 721 } else {
Mircea3M 0:bf14022d00d5 722 showMessage(POSITION_OVERFLOW);
Mircea3M 0:bf14022d00d5 723 }
Mircea3M 0:bf14022d00d5 724 }
Mircea3M 0:bf14022d00d5 725
Mircea3M 0:bf14022d00d5 726 if(!BUT_Portocaliu && position_index < 31) {
Mircea3M 0:bf14022d00d5 727 position_index++; // increase actual increment
Mircea3M 0:bf14022d00d5 728 }
Mircea3M 0:bf14022d00d5 729 if(!BUT_Maro && position_index > 0) {
Mircea3M 0:bf14022d00d5 730 position_index--; // decrease actual increment
Mircea3M 0:bf14022d00d5 731 }
Mircea3M 0:bf14022d00d5 732
Mircea3M 0:bf14022d00d5 733 if(!BUT_Verde) { // record axis position
Mircea3M 0:bf14022d00d5 734 number_of_mem_positions++;
Mircea3M 0:bf14022d00d5 735 memorised_positions[number_of_mem_positions] = position;
Mircea3M 0:bf14022d00d5 736 showMessage(POSITION_RECORDED);
Mircea3M 0:bf14022d00d5 737 }
Mircea3M 0:bf14022d00d5 738
Mircea3M 0:bf14022d00d5 739 if(!BUT_Rosu) { // end free axis jog
Mircea3M 0:bf14022d00d5 740 showMessage(ESC);
Mircea3M 0:bf14022d00d5 741 jog_finished = true;
Mircea3M 0:bf14022d00d5 742 }
Mircea3M 0:bf14022d00d5 743
Mircea3M 0:bf14022d00d5 744 if(move_forward || move_backward) { // move axis forward or backward accordingly to the coresponding actual index position
Mircea3M 0:bf14022d00d5 745 LECP_move_axis();
Mircea3M 0:bf14022d00d5 746 }
Mircea3M 0:bf14022d00d5 747 }
Mircea3M 0:bf14022d00d5 748 }
Mircea3M 0:bf14022d00d5 749
Mircea3M 0:bf14022d00d5 750 /*** CHECK FOR LCEP ALARMS ***/
Mircea3M 0:bf14022d00d5 751 bool LECP_alarms()
Mircea3M 0:bf14022d00d5 752 {
Mircea3M 0:bf14022d00d5 753 // check for alarms
Mircea3M 0:bf14022d00d5 754 if(DI_B12_ESTOP || DI_B13_ALARM) return 1;
Mircea3M 0:bf14022d00d5 755 else return 0;
Mircea3M 0:bf14022d00d5 756 }
Mircea3M 0:bf14022d00d5 757
Mircea3M 0:bf14022d00d5 758 /*** RESET LCEP ALARMS ***/
Mircea3M 0:bf14022d00d5 759 void LECP_alarms_rst()
Mircea3M 0:bf14022d00d5 760 {
Mircea3M 0:bf14022d00d5 761 DO_A12_RESET = true;
Mircea3M 0:bf14022d00d5 762 wait(0.5);
Mircea3M 0:bf14022d00d5 763 DO_A12_RESET = false;
Mircea3M 0:bf14022d00d5 764 }
Mircea3M 0:bf14022d00d5 765
Mircea3M 0:bf14022d00d5 766 /*** CHECK IF SVRE BIT BECAME ACTIVE AFTER SERVO ON CMD ***/
Mircea3M 0:bf14022d00d5 767 bool LECP6_SVRE()
Mircea3M 0:bf14022d00d5 768 {
Mircea3M 0:bf14022d00d5 769 // check if SVRE bit is on as an action to Servo ON
Mircea3M 0:bf14022d00d5 770 int timeout = 0;
Mircea3M 0:bf14022d00d5 771 do {
Mircea3M 0:bf14022d00d5 772 //wait(0.1);
Mircea3M 0:bf14022d00d5 773 wait_ms(50);
Mircea3M 0:bf14022d00d5 774 timeout++;
Mircea3M 0:bf14022d00d5 775 if(timeout >= 10) return 0;
Mircea3M 0:bf14022d00d5 776 } while(DI_B11_SVRE);
Mircea3M 0:bf14022d00d5 777 return 1;
Mircea3M 0:bf14022d00d5 778 }
Mircea3M 0:bf14022d00d5 779
Mircea3M 0:bf14022d00d5 780 /*** CHECK IF LCPE IS BUSY PERFORMING AN OPERATION ***/
Mircea3M 0:bf14022d00d5 781 bool LECP6_BUSY()
Mircea3M 0:bf14022d00d5 782 {
Mircea3M 0:bf14022d00d5 783 int timeout = 0;
Mircea3M 0:bf14022d00d5 784 do {
Mircea3M 0:bf14022d00d5 785 wait(0.1);
Mircea3M 0:bf14022d00d5 786 timeout++;
Mircea3M 0:bf14022d00d5 787 if(timeout >= 10) return 1;
Mircea3M 0:bf14022d00d5 788 } while(!DI_B7_BUSY);
Mircea3M 0:bf14022d00d5 789 return 0;
Mircea3M 0:bf14022d00d5 790 }
Mircea3M 0:bf14022d00d5 791