z ysaito
https://os.mbed.com/users/sayzyas
Dependencies: QEI TextLCD mbed
Revision 0:73dd48be5ca6, committed 2018-07-26
- Comitter:
- sayzyas
- Date:
- Thu Jul 26 00:20:15 2018 +0000
- Commit message:
- 2018.07.26
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/P1_1Y_main.cpp Thu Jul 26 00:20:15 2018 +0000
@@ -0,0 +1,364 @@
+/**********************************************************
+ * Project: B2Dash (1F-1)
+ * Title: CrExp B2D Resolver Ctrl Main
+ * Target: LPCXpresso824-Max
+ * Author: zStranger
+ * Date(Latest update) 2017.8.23
+ * --------------------------------------------------------
+ * Article
+ *
+ *
+ * --------------------------------------------------------
+ *
+ * LPCXpresso 824-MAX
+ * +---------USB---------+
+ * | |
+ * | |
+ * | |
+ * | |
+ * | | SCL P0_10 D15 -> IIC SCL
+ * | ## ### ## | SDA P0_11 D14 -> IIC SDA
+ * | # # # # # # | AVDD
+ * N/A | # # # # # | GND
+ * +3V3 | ## # # # | SCK P0_24 D13
+ * NRST | # # # # # | MISO P0_25 D12
+ * +3V3 | # # # # ###### | MOSI P0_26 D11 -> Res B
+ * +5V | ## ##### # | SSEL P0_15 D10 -> Res A
+ * GND | | P0_27 D9 --> SW ZERO
+ * GND | | P0_13 D8 --> Int2Main
+ * N/A | | P0_17 D7
+ * | | P0_16 D6
+ * mtrcnt (Dram) --> A0 P0_6 | | PWM P0_28 D5 --> LCD DB7
+ * mtrcnt (Cable) --> A1 P0_14 | | PWM P0_18 D4 --> LCD DB6
+ * mtrspd (Dram CW) -> A2 P0_23 | | PWM P0_12 D3 --> LCD DB5
+ * mtrspd (Dram CCW) -> A3 P0_22 | | PWM P0_19 D2 --> LCD DB4
+ * mtrspd (Cable CW) -> A4 P0_21 | | TX P0_4 D1 --> LCD E
+ * mtrspd (Cable CCW)-> A5 P0_20 | | RX P0_0 D0 --> LCD RS
+ * +---------------------+
+ *
+ ***************************************/
+
+#include "mbed.h"
+//#include "rtos.h"
+#include "QEI.h"
+#include "common.h"
+#include "debugprint.h"
+#include <math.h>
+#include "mcchk.h"
+#include "TextLCD.h"
+
+// Hardware setting
+
+Serial pc(USBTX, USBRX); // tx, rx
+
+/*
+QEI wheel
+ PinName channelA,
+ PinName channelB,
+ PinName index,
+ int pulsesPerRev,
+ Encoding encoding) : channelA_(channelA), channelB_(channelB),
+ index_(index)
+*/
+#ifdef ResolverDirection_f // For Fix Fallong Winch
+//QEI wheel( P0_26, P0_15, NC, ROTATE_PER_RESOLUTION, QEI::X2_ENCODING );
+QEI wheel( D11, D10, NC, ROTATE_PER_RESOLUTION, QEI::X2_ENCODING );
+#else //For Falling Position Moving Winch
+//QEI wheel( P0_15, P0_26, NC, ROTATE_PER_RESOLUTION, QEI::X2_ENCODING );
+QEI wheel( D10, D11, NC, ROTATE_PER_RESOLUTION, QEI::X2_ENCODING );
+
+#endif // FFWinchPhaseSetting
+
+//TextLCD lcd(P0_28, P0_18, P0_12, P0_19, P0_4, P0_0); // rs, e, d4-d7
+TextLCD lcd(D0,D1,D2,D5,D12,D13); // rs, e, d4-d7
+
+/*
+ Dram r = 60mm
+ Cable fai = 3
+
+ (60+3)*3.14 = 197.82mm ==> 2^12*4 = 4096 * 4 = 16384 pulse
+ 1(mm) = 82.82(pulse)
+ 0.01208(mm) = 1(pulse)
+
+*/
+DigitalOut led1(LED1); // Red
+DigitalOut led2(LED2); // Green
+DigitalOut led3(LED3); // Blue
+
+DigitalOut int2hst(D8);
+DigitalIn sw_zero(D7);
+
+AnalogIn mtcnt_wdram(A0);
+AnalogIn mtcnt_wcabl(A1);
+AnalogIn mtspd_wdram_cw(A2);
+AnalogIn mtspd_wdram_ccw(A3);
+AnalogIn mtspd_wcabl_cw(A4);
+AnalogIn mtspd_wcabl_ccw(A5);
+
+I2CSlave slave(D14, D15); //I2C SDA SCL
+
+//Mutex lcdMutex;
+
+void led_demo(){
+ int i;
+ for( i = 0; i < 20; i++ ) {
+ led1 = LED_ON;
+ led2 = LED_OFF;
+ led3 = LED_OFF;
+ wait_ms(20);
+ led1 = LED_OFF;
+ led2 = LED_OFF;
+ led3 = LED_OFF;
+ wait_ms(20);
+ led1 = LED_OFF;
+ led2 = LED_ON;
+ led3 = LED_OFF;
+ wait_ms(20);
+ led1 = LED_OFF;
+ led2 = LED_OFF;
+ led3 = LED_OFF;
+ wait_ms(20);
+ led1 = LED_OFF;
+ led2 = LED_OFF;
+ led3 = LED_ON;
+ wait_ms(20);
+ led1 = LED_OFF;
+ led2 = LED_OFF;
+ led3 = LED_OFF;
+ wait_ms(20);
+ }
+// wait_ms(3000);
+}
+
+void lcd_dsp( int column, int row, char* msg, int cnt)
+{
+ for( int i = 0; i < cnt; i++ )
+ {
+ lcd.locate(column+i,row);
+ lcd.putc(*msg++);
+ }
+ wait_ms(1);
+// Thread::wait(10);
+}
+
+void lcd_out( char* line1, char* line2 ) {
+ lcd.cls();
+ lcd.locate(0, 0);
+ lcd.printf(line1);
+ lcd.locate(0, 1);
+ lcd.printf(line2);
+}
+
+
+
+// ========================================================================
+// Main Function
+// ========================================================================
+int main() {
+ int i = 0;
+ int ii;
+ char msg[16] = "";
+ char buf[12]; // command buffer
+
+ int pulse;
+ double dropAmt_d = 0.0f;
+ int16_t dropAmt;
+ int16_t pos_offset = 0;;
+
+ uint16_t dram_diameter;
+ uint16_t dis_correct_value;
+ uint8_t rresolution;
+
+ bool flg_mtrEStop = false;
+
+ float mc_wdram; // motor current
+ float mc_wcabl; // motor current
+ int16_t sp_wdram_f; // motor speed
+ int16_t sp_wdram_r; // motor speed
+ int16_t sp_wcabl_f; // motor speed
+ int16_t sp_wcabl_r; // motor speed
+
+ mcchk mcc;
+ sw_zero.mode(PullUp);
+ pc.baud(115200);
+ slave.address(I2C_ADDRESS_RESOLVER);
+ //led_demo();
+
+ // 1234567890123456 1234567890123456
+ lcd_dsp(0,0,"B2' FFWinchProto",16);
+ lcd_dsp(0,1,"Revast Co.,Ltd. ",16);
+ wait_ms(1000);
+ lcd_dsp(0,1,"Now Booting ... ",16);
+
+ DEBUG_PRINT_L0("\r\n");
+ DEBUG_PRINT_L0("LPC824> +-------------------------------------------------------------\r\n");
+ DEBUG_PRINT_L0("LPC824> | Project: B2Dash Debris Explorer Winch test machine\r\n");
+ DEBUG_PRINT_L0("LPC824> |-------------------------------------------------------------\r\n");
+ DEBUG_PRINT_L0("LPC824> | This is: Resolver pulse counter Main\r\n");
+ DEBUG_PRINT_L0("LPC824> | Target MCU: mbed LPC824MAX\r\n");
+ DEBUG_PRINT_L0("LPC824> | Letest update: %s\r\n", LatestUpDate);
+ DEBUG_PRINT_L0("LPC824> | Program Revision: %s\r\n", ProgramRevision);
+ DEBUG_PRINT_L0("LPC824> | Author: %s\r\n", Author);
+ DEBUG_PRINT_L0("LPC824> | Copyright(C) 2017 %s Allright Reserved\r\n", Company);
+ DEBUG_PRINT_L0("LPC824> +-------------------------------------------------------------\r\n");
+
+ wheel.reset();
+
+ wait_ms(1000);
+ lcd_dsp(0,1,"Wait net connect",16);
+ DEBUG_PRINT_L0("LPC824> wait ... \r\n");
+
+ // Thread roop
+ while(1){
+ DEBUG_PRINT_L0("LPC824> x");
+
+ // When push winch position zero reset button then whell reset
+ if( sw_zero == 0 )
+ {
+ wheel.reset();
+ wait_ms(1);
+ }
+
+ /* [ Automatic stop function control ] **************************************
+ * Down(CW)
+ * When camera head reached to MAX_DROP_AMOUNT (= 3500 mm)
+ * then the winch will stop owing to protect system breake.
+ * UP(CCW) Master (= host) shuld always send any IIC packet to client
+ * When camera head reached to home position (= 0000 mm)
+ * then the winch will stop owing to protect system breake.
+ * DROP_AMOUNT_ADJ value is adjustment value for over and under shooting.
+ * ************************************************************************* */
+ if(
+ (dropAmt>=( MAX_DROP_AMOUNT - DROP_AMOUNT_ADJ ))&&(flg_mtrEStop==false) ||
+ (dropAmt<= DROP_AMOUNT_ADJ )&&(flg_mtrEStop==false)
+ ){
+ // DEBUG_PRINT_L1("#### Interuppt to host ####\r\n");
+ flg_mtrEStop = true;
+ int2hst = 1; // interrupt to host controller
+ }
+ else
+ {
+ // DEBUG_PRINT_L1("#### Interuppt off ####\r\n");
+ int2hst = 0; // interrupt off
+ }
+
+ if(
+ ( dropAmt < ( MAX_DROP_AMOUNT - DROP_AMOUNT_ADJ ) ) &&
+ ( dropAmt > DROP_AMOUNT_ADJ )
+ ){
+ flg_mtrEStop = false;
+ }
+
+ /* ** Note ** ****************************************************** */
+ /* This is Slave (= Client) */
+ /* Master (= host) shuld always send any IIC packet to client */
+ /* Because slave is waiting packet here ! */
+ /* ***************************************************************** */
+ i = slave.receive();
+ slave.read(buf, NumberOfI2CCommand);
+
+ switch (i) {
+ case I2CSlave::NoData:
+ // DEBUG_PRINT_L1("the slave has not been addressed\r\n");
+ // read_motor_current(2);
+ break;
+
+ case I2CSlave::ReadAddressed:
+
+ pulse = wheel.getPulses(); /* Master read the current position of winch here! */
+ /* dropAmt calculation */
+ dropAmt_d = ( pulse * ( (double)( ((double)dram_diameter/(double)100) * (double)PAI / (pow(2.0, (double)rresolution)*4 ) ) ) );
+ dropAmt = (int16_t)(dropAmt_d * (double)((double)dis_correct_value/(double)dis_correct_value));
+ dropAmt += pos_offset;
+ dropAmt *= -1;
+ DEBUG_PRINT_L2("LPC824> Pulse: %07d, DropAmount(Offset): %04d(%4d) mm\t (%d, %d)", pulse, dropAmt, pos_offset, dram_diameter, rresolution);
+
+ mc_wdram = mtcnt_wdram.read();
+ mc_wcabl = mtcnt_wcabl.read();
+ sp_wdram_f = (int16_t)(mtspd_wdram_cw.read()*100.0f);
+ sp_wdram_r = (int16_t)(mtspd_wdram_ccw.read()*100.0f);
+ sp_wcabl_f = (int16_t)(mtspd_wcabl_cw.read()*100.0f);
+ sp_wcabl_r = (int16_t)(mtspd_wcabl_ccw.read()*100.0f);
+ // DEBUG_PRINT_L2("Motor speed: Dram CW=%03d CCW=%03d Cable CW=%03d CCW=%03d \t", sp_wdram_f, sp_wdram_r, sp_wcabl_f, sp_wcabl_r);
+ // DEBUG_PRINT_L2("Motor current: Dram=%f Cable=%f \r\n", mc_wdram, mc_wcabl);
+
+ sprintf(msg,"[%04d mm]*", dropAmt);
+ lcd_dsp(3,1," ",4);
+ lcd_dsp(7,1,msg,9);
+
+ msg[0] = 0x12; // Dummy data for data check
+ msg[1] = sp_wdram_f & 0xFF;
+ msg[2] = (sp_wdram_f >> 8) & 0xFF;
+ msg[3] = sp_wdram_r & 0xFF;
+ msg[4] = (sp_wdram_r >> 8) & 0xFF;
+ msg[5] = sp_wcabl_f & 0xFF;
+ msg[6] = (sp_wcabl_f >> 8) & 0xFF;
+ msg[7] = sp_wcabl_r & 0xFF;
+ msg[8] = (sp_wcabl_r >> 8) & 0xFF;
+ msg[9] = dropAmt & 0xFF;
+ msg[10] = (dropAmt >> 8) & 0xFF;
+ msg[11] = 0x34; // Dummy data
+ slave.write(msg, 12); // Includes null char
+ //DEBUG_PRINT_L2("\tGot data : %02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x\r\n", msg[1],msg[2],msg[3],msg[4],msg[5],msg[6],msg[7],msg[8]);
+ // lcd_dsp(3,1,">",1);
+ break;
+
+ case I2CSlave::WriteAddressed:
+ if( buf[I2C_CP_COMMAND] == 'Z'){
+ // DEBUG_PRINT_L1("Z got\r\n");
+ sprintf(msg," %03d", sp_wcabl_f);
+ lcd_dsp(0,0,msg,4);
+ sprintf(msg," %03d", sp_wcabl_r);
+ lcd_dsp(4,0,msg,4);
+ sprintf(msg," %03d", sp_wdram_r);
+ lcd_dsp(8,0,msg,4);
+ sprintf(msg," %03d", sp_wdram_f);
+ lcd_dsp(12,0,msg,4);
+ dram_diameter = ( buf[I2C_CP_WDRAM_DIA_X100_UPPER] << 8 );
+ dram_diameter |= buf[I2C_CP_WDRAM_DIA_X100_LOWER];
+ dis_correct_value = ( buf[I2C_CP_ADJUST_VALUE_X10000_UPPER] << 8 );
+ dis_correct_value |= buf[I2C_CP_ADJUST_VALUE_X10000_LOWER];
+ rresolution = buf[I2C_CP_RESOLVER_RESO];
+ /*
+ DEBUG_PRINT_L1("Bd4> ===========================================\r\n");
+ DEBUG_PRINT_L1("Bd4> Dram Diameter : %d\t(mm)\r\n", dram_diameter);
+ DEBUG_PRINT_L1("Bd4> CCable Diameter : %d\t(mm)\r\n", dis_correct_value);
+ DEBUG_PRINT_L1("Bd4> Resolver Resolution : %d\t(bit)\r\n", rresolution);
+ DEBUG_PRINT_L1("Bd4> -------------------------------------------\r\n", rresolution);
+ DEBUG_PRINT_L1("Bd4> Real Diameter : %d\t(mm)\r\n", (dram_diameter+dis_correct_value));
+ DEBUG_PRINT_L1("Bd4> Rotation Pulse / 1round : %d\t(pulse)\r\n", (int)(pow(2.0, (double)rresolution)*4));
+ DEBUG_PRINT_L1("Bd4> Distance / 1pulse : %lf\t(mm)\r\n", (double)( ((dram_diameter+dis_correct_value)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) );
+ DEBUG_PRINT_L1("Bd4> ===========================================\r\n");
+ */
+ }
+ else if( buf[I2C_CP_COMMAND] == 'N'){
+ sprintf(msg," %03d", sp_wcabl_f);
+ lcd_dsp(0,0,msg,4);
+ sprintf(msg," %03d", sp_wcabl_r);
+ lcd_dsp(4,0,msg,4);
+ sprintf(msg," %03d", sp_wdram_r);
+ lcd_dsp(8,0,msg,4);
+ sprintf(msg," %03d", sp_wdram_f);
+ lcd_dsp(12,0,msg,4);
+ }
+ // else if( buf[I2C_CP_COMMAND] == 'M'){
+ // lcd_dsp(4,0,"Winch manual",12);
+ // }
+ break;
+ }
+ // Animation
+ if( ii < 5 ){
+ lcd_dsp(0,1,"[|]",3);
+ }
+ else if( ii < 10 ){
+ lcd_dsp(0,1,"[/]",3);
+ }
+ else if ( ii < 15 ){
+ lcd_dsp(0,1,"[-]",3);
+ }
+ ii++;
+ if( ii >= 15 ) ii = 0;
+ wait_ms(5);
+ }
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Thu Jul 26 00:20:15 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TextLCD.lib Thu Jul 26 00:20:15 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/TextLCD/#308d188a2d3a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/common.h Thu Jul 26 00:20:15 2018 +0000
@@ -0,0 +1,94 @@
+/* Information */
+#define LatestUpDate "2017.08.30"
+#define ProgramRevision "Rev 2.30"
+#define Author "Y.Saito(zinsor)"
+#define Company "Revast Co.,Ltd"
+
+
+/* ****************************************** */
+/* Motor */
+/* ****************************************** */
+enum{
+ MOTOR_RFCRW, // RF crawler
+ MOTOR_LBCRW, // LB crawler
+ MOTOR_RFTFM, // RF Transform
+ MOTOR_LBTFM, // LB transform
+ MOTOR_WINCH, // Winch
+ MOTOR_CMPAN, // Pan
+ MOTOR_CTILT, // Tilt
+ MOTOR_CABLE // Cable transfar
+};
+
+enum{
+ MOTOR_DIR_FWD, // Motor forwaed rotation
+ MOTOR_DIR_RVS, // Motor reverse rotation
+ MOTOR_DIR_STP // Motor stop
+};
+
+
+enum{
+ MOTOR_NO0,
+ MOTOR_NO1,
+ MOTOR_NO2,
+ MOTOR_NO3
+};
+
+/* ****************************************** */
+/* I2C */
+/* ****************************************** */
+#define NumberOfI2CCommand 12
+enum{
+ I2C_CP_COMMAND, // instruction command
+ I2C_CP_MOTORNO, // motor number
+ I2C_CP_M_DIR, // motor rotation direction
+ I2C_CP_M_CNTTH_U, // motor current limit detection threshold upper byte
+ I2C_CP_M_CNTTH_L, // motor current limit detection threshold lower byte
+ I2C_CP_WDRAM_DIA_X100_UPPER, // winch dram motor diameter upper
+ I2C_CP_WDRAM_DIA_X100_LOWER, // winch dram motor diameter lower
+ I2C_CP_ADJUST_VALUE_X10000_UPPER, // cable diameter upper byte
+ I2C_CP_ADJUST_VALUE_X10000_LOWER, // cable diameter lower byte
+ I2C_CP_RESOLVER_RESO, // resolver resolution (bit)
+ I2C_CP_PRESET_CPOS_UPPER, // preset position upper
+ I2C_CP_PRESET_CPOS_LOWER // preset position lower
+};
+
+#define I2C_ADDRESS_RESOLVER 0x02
+
+
+/* ****************************************** */
+/* Parameters */
+/* ****************************************** */
+#define MAX_DROP_AMOUNT 3495 /* Maxmun dropping distance : ex 3500 (mm) */
+#define DROP_AMOUNT_ADJ 10 /* Maxmun dropping distance : ex 3500 (mm) */
+
+#define CALIBRATION_COUNT 300 /* Joystick calibration count */
+#define DEAD_BAND 5 /* Joystick dead band wide */
+
+#define ROTATE_PER_RESOLUTION 24 /* For QEI */
+#define PAI 3.14159265359
+
+#define LED_ON 0
+#define LED_OFF 1
+
+
+/* ****************************************** */
+/* Structure */
+/* ****************************************** */
+/* Winch setting value */
+typedef struct {
+ uint16_t cth_dram_mtr_f; // 2 Current threshold: winch dram Motor ( forward )
+ uint16_t cth_dram_mtr_r; // 2 Current threshold: winch dram Motor ( reverse )
+ uint16_t cth_cabl_mtr_f; // 2 Current threshold: winch cable Motor ( forward )
+ uint16_t cth_cabl_mtr_r; // 2 Current threshold: winch cable Motor ( reverse )
+ uint16_t dram_dmtr_x100; // 2 Winch dram diameter x 100
+ uint16_t adj_val_x10000; // 2 Winch adjust value x 100
+ uint8_t res_resolution; // 2 Winch resolver resolution (bit)
+ uint8_t reserved_1; // 1 reserved for future use
+ uint8_t reserved_2; // 1 reserved for future use
+ uint8_t reserved_3; // 1 reserved for future use
+} wch_SetValue_t;
+
+typedef struct SetValue {
+ wch_SetValue_t wchCtrl;
+} setValue_t;
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/debugprint.h Thu Jul 26 00:20:15 2018 +0000 @@ -0,0 +1,55 @@ +// ====================================================================== +// For Debugging +// ====================================================================== +#define __DEBUG_PRINT_SW__ // Display SW Status to console +#define __DEBUG_L0__ +#define __DEBUG_L1__ +#define __DEBUG_L2__ +#define __DEBUG_L3__ +//#define __DEBUG_L4__ +//#define __DEBUG_L5__ + +#define __DEBUG_WINCH_DATA__ + +#ifdef __DEBUG_WINCH_DATA__ + #define DEBUG_PRINT_WINCH_DATA(...) pc.printf(__VA_ARGS__) +#else + #define DEBUG_PRINT_WINCH_DATA(...) +#endif + +#ifdef __DEBUG_PRINT_SW__ + #define DEBUG_PRINT_SW(...) pc.printf(__VA_ARGS__) +#else + #define DEBUG_PRINT_SW(...) +#endif + +#ifdef __DEBUG_L0__ + #define DEBUG_PRINT_L0(...) pc.printf(__VA_ARGS__) +#else + #define DEBUG_PRINT_L0(...) +#endif +#ifdef __DEBUG_L1__ + #define DEBUG_PRINT_L1(...) pc.printf(__VA_ARGS__) +#else + #define DEBUG_PRINT_L1(...) +#endif +#ifdef __DEBUG_L2__ + #define DEBUG_PRINT_L2(...) pc.printf(__VA_ARGS__) +#else + #define DEBUG_PRINT_L2(...) +#endif +#ifdef __DEBUG_L3__ + #define DEBUG_PRINT_L3(...) pc.printf(__VA_ARGS__) +#else + #define DEBUG_PRINT_L3(...) +#endif +#ifdef __DEBUG_L4__ + #define DEBUG_PRINT_L4(...) pc.printf(__VA_ARGS__) +#else + #define DEBUG_PRINT_L4(...) +#endif +#ifdef __DEBUG_L5__ + #define DEBUG_PRINT_L5(...) pc.printf(__VA_ARGS__) +#else + #define DEBUG_PRINT_L5(...) +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Jul 26 00:20:15 2018 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/mbed/builds/e2bfab296f20 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/mcchk.cpp Thu Jul 26 00:20:15 2018 +0000
@@ -0,0 +1,536 @@
+#include "mbed.h"
+#include "common.h"
+#include <math.h>
+#include "mcchk.h"
+
+
+// Motor current
+AnalogIn mctr3_m2c(A5); // LB crawler
+AnalogIn mctr3_m1c(A4); // RF crawler
+AnalogIn mctr2_m2c(A3); // LB transform
+AnalogIn mctr2_m1c(A2); // RF Transform
+AnalogIn mctr1_m2c(A1); // Tilt
+AnalogIn mctr1_m1c(A0); // B1: Pan, B2:Winch
+
+
+mcchk::mcchk()
+{
+ cnt_mclock_LBCRW_f = 0; // motor lock counter
+ cnt_mclock_LBCRW_r = 0; // motor lock counter
+ cnt_mclock_RFCRW_f = 0; // motor lock counter
+ cnt_mclock_RFCRW_r = 0; // motor lock counter
+ cnt_mclock_LBTFM_f = 0; // motor lock counter
+ cnt_mclock_LBTFM_r = 0; // motor lock counter
+ cnt_mclock_RFTFM_f = 0; // motor lock counter
+ cnt_mclock_RFTFM_r = 0; // motor lock counter
+ cnt_mclock_CMPAN_f = 0; // motor lock counter
+ cnt_mclock_CMPAN_r = 0; // motor lock counter
+ cnt_mclock_CTILT_f = 0; // motor lock counter
+ cnt_mclock_CTILT_r = 0; // motor lock counter
+ cnt_mclock_WINCH_f = 0; // motor lock counter
+ cnt_mclock_WINCH_r = 0; // motor lock counter
+}
+
+
+// --------------------------------------------------------------
+// Motor Current Calibration Function
+// Arg: Motor Number 1 - 3
+// --------------------------------------------------------------
+void mcchk::set_init_mc( int cnt )
+{
+
+ m_LBCRW_center_value = 0;
+ m_RFCRW_center_value = 0;
+ m_LBTFM_center_value = 0;
+ m_RFTFM_center_value = 0;
+ m_WINCH_center_value = 0;
+ m_CMPAN_center_value = 0;
+ m_CTILT_center_value = 0;
+
+ for( int i = 0; i < cnt; i++ )
+ {
+ m_LBCRW_center_value += mctr3_m1c.read() * 100.0f;
+ m_RFCRW_center_value += mctr3_m2c.read() * 100.0f;
+ m_LBTFM_center_value += mctr2_m1c.read() * 100.0f;
+ m_RFTFM_center_value += mctr2_m2c.read() * 100.0f;
+ m_WINCH_center_value += mctr1_m1c.read() * 100.0f;
+ m_CMPAN_center_value += mctr1_m1c.read() * 100.0f;
+ m_CTILT_center_value += mctr1_m2c.read() * 100.0f;
+ }
+
+ m_LBCRW_center_value /= cnt;
+ m_RFCRW_center_value /= cnt;
+ m_LBTFM_center_value /= cnt;
+ m_RFTFM_center_value /= cnt;
+ m_WINCH_center_value /= cnt;
+ m_CMPAN_center_value /= cnt;
+ m_CTILT_center_value /= cnt;
+}
+
+
+float mcchk::rd_motorCurrent( int no )
+{
+ if( no == 11 ) return mctr1_m1c.read()*100.0f;
+ else if( no == 12 ) return mctr1_m2c.read()*100.0f;
+ else if( no == 21 ) return mctr2_m1c.read()*100.0f;
+ else if( no == 22 ) return mctr2_m2c.read()*100.0f;
+ else if( no == 31 ) return mctr3_m1c.read()*100.0f;
+ else if( no == 32 ) return mctr3_m2c.read()*100.0f;
+ return 0;
+}
+
+
+bool mcchk::rdnchk_motorCurrent(
+ int8_t motor_number,
+ int8_t motor_dir,
+ int8_t motor_lock_chk_cnt
+){
+ bool mcchk = false;
+ bool rts = false;
+
+ // Check RF-Crawler
+ if( motor_number == MOTOR_RFCRW )
+ {
+ if( motor_dir == MOTOR_DIR_FWD )
+ {
+ mcchk = chk_motor_lock( mctr3_m1c.read()*100.0f, m_RFCRW_center_value, mc_th_RFCRW_f);
+ if( mcchk == true )
+ {
+ cnt_mclock_RFCRW_f += 1;
+ }
+ else
+ { if( cnt_mclock_RFCRW_f > 0 )
+ {
+ cnt_mclock_RFCRW_f -= 1;
+ }
+ else
+ {
+ cnt_mclock_RFCRW_f = 0;
+ }
+ }
+ if( cnt_mclock_RFCRW_f > motor_lock_chk_cnt )
+ {
+ flg_mclock_RFCRW = 1;
+ }
+ else
+ {
+ flg_mclock_RFCRW = 0;
+ rts = false; // NO LOCK
+ }
+ }
+ else
+ {
+ mcchk = chk_motor_lock( mctr3_m1c.read()*100.0f, m_RFCRW_center_value, mc_th_RFCRW_r);
+ if( mcchk == true )
+ {
+ cnt_mclock_RFCRW_r += 1;
+ }
+ else
+ { if( cnt_mclock_RFCRW_r > 0 )
+ {
+ cnt_mclock_RFCRW_r -= 1;
+ }
+ else
+ {
+ cnt_mclock_RFCRW_r = 0;
+ }
+ }
+ if( cnt_mclock_RFCRW_r > motor_lock_chk_cnt )
+ {
+ flg_mclock_RFCRW = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_RFCRW = 0;
+ rts = false; // NO LOCK
+ }
+ }
+ }
+ // Check LB-Crawler
+ else if( motor_number == MOTOR_LBCRW )
+ {
+ if( motor_dir == MOTOR_DIR_FWD )
+ {
+ mcchk = chk_motor_lock( mctr3_m2c.read()*100.0f, m_LBCRW_center_value, mc_th_LBCRW_f);
+ if( mcchk == true )
+ {
+ cnt_mclock_LBCRW_f += 1;
+ }
+ else
+ { if( cnt_mclock_LBCRW_f > 0 )
+ {
+ cnt_mclock_LBCRW_f -= 1;
+ }
+ else
+ {
+ cnt_mclock_LBCRW_f = 0;
+ }
+ }
+ if( cnt_mclock_LBCRW_f > motor_lock_chk_cnt )
+ {
+ flg_mclock_LBCRW = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_LBCRW = 0;
+ rts = false; // LOCK
+ }
+ }
+ else
+ {
+ mcchk = chk_motor_lock( mctr3_m2c.read()*100.0f, m_LBCRW_center_value, mc_th_LBCRW_r);
+ if( mcchk == true )
+ {
+ cnt_mclock_LBCRW_r += 1;
+ }
+ else
+ { if( cnt_mclock_LBCRW_r > 0 )
+ {
+ cnt_mclock_LBCRW_r -= 1;
+ }
+ else
+ {
+ cnt_mclock_LBCRW_r= 0;
+ }
+ }
+ if( cnt_mclock_LBCRW_r > motor_lock_chk_cnt )
+ {
+ flg_mclock_LBCRW = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_LBCRW = 0;
+ rts = false; // LOCK
+ }
+ }
+ }
+ // Check RF-Crawler
+ else if( motor_number == MOTOR_RFTFM )
+ {
+ if( motor_dir == MOTOR_DIR_FWD )
+ {
+ mcchk = chk_motor_lock( mctr2_m1c.read()*100.0f, m_RFTFM_center_value, mc_th_RFTFM_f);
+ if( mcchk == true )
+ {
+ cnt_mclock_RFTFM_f += 1;
+ }
+ else
+ { if( cnt_mclock_RFTFM_f > 0 )
+ {
+ cnt_mclock_RFTFM_f -= 1;
+ }
+ else
+ {
+ cnt_mclock_RFTFM_f = 0;
+ }
+ }
+ if( cnt_mclock_RFTFM_f > motor_lock_chk_cnt )
+ {
+ flg_mclock_RFTFM = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_RFTFM = 0;
+ rts = false; // LOCK
+ }
+ }
+ else
+ {
+ rts = chk_motor_lock( mctr2_m1c.read()*100.0f, m_RFTFM_center_value, mc_th_RFTFM_r);
+ if( rts == true )
+ {
+ cnt_mclock_RFTFM_r += 1;
+ }
+ else
+ { if( cnt_mclock_RFTFM_r > 0 )
+ {
+ cnt_mclock_RFTFM_r -= 1;
+ }
+ else
+ {
+ cnt_mclock_RFTFM_r = 0;
+ }
+ }
+ if( cnt_mclock_RFTFM_r > motor_lock_chk_cnt )
+ {
+ flg_mclock_RFTFM = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_RFTFM = 0;
+ rts = false; // LOCK
+ }
+ }
+ }
+ // Check LB-Transform
+ else if( motor_number == MOTOR_LBTFM )
+ {
+ if( motor_dir == MOTOR_DIR_FWD )
+ {
+ mcchk = chk_motor_lock( mctr2_m2c.read()*100.0f, m_LBTFM_center_value, mc_th_LBTFM_f);
+ if( mcchk == true )
+ {
+ cnt_mclock_LBTFM_f += 1;
+ }
+ else
+ { if( cnt_mclock_LBTFM_f > 0 )
+ {
+ cnt_mclock_LBTFM_f -= 1;
+ }
+ else
+ {
+ cnt_mclock_LBTFM_f = 0;
+ }
+ }
+ if( cnt_mclock_LBTFM_f > motor_lock_chk_cnt )
+ {
+ flg_mclock_LBTFM = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_LBTFM = 0;
+ rts = false; // LOCK
+ }
+ }
+ else
+ {
+ mcchk = chk_motor_lock( mctr2_m2c.read()*100.0f, m_LBTFM_center_value, mc_th_LBTFM_r);
+ if( mcchk == true )
+ {
+ cnt_mclock_LBTFM_r += 1;
+ }
+ else
+ { if( cnt_mclock_LBTFM_r > 0 )
+ {
+ cnt_mclock_LBTFM_r -= 1;
+ }
+ else
+ {
+ cnt_mclock_LBTFM_r = 0;
+ }
+ }
+ if( cnt_mclock_LBTFM_r > motor_lock_chk_cnt )
+ {
+ flg_mclock_LBTFM = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_LBTFM = 0;
+ rts = false; // LOCK
+ }
+ }
+ }
+
+ // Check Winch
+ else if( motor_number == MOTOR_WINCH )
+ {
+ if( motor_dir == MOTOR_DIR_FWD )
+ {
+ mcchk= chk_motor_lock( mctr1_m1c.read()*100.0f, m_WINCH_center_value, mc_th_WINCH_f);
+ if( mcchk == true )
+ {
+ cnt_mclock_WINCH_f += 1;
+ }
+ else
+ { if( cnt_mclock_WINCH_f > 0 )
+ {
+ cnt_mclock_WINCH_f -= 1;
+ }
+ else
+ {
+ cnt_mclock_WINCH_f = 0;
+ }
+
+ }
+ if( cnt_mclock_WINCH_f > motor_lock_chk_cnt )
+ {
+ flg_mclock_WINCH = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_WINCH = 0;
+ rts = false; // LOCK
+ }
+ }
+ else
+ {
+ mcchk = chk_motor_lock( mctr1_m1c.read()*100.0f, m_WINCH_center_value, mc_th_WINCH_r);
+ if( mcchk == true )
+ {
+ cnt_mclock_WINCH_r += 1;
+ }
+ else
+ { if( cnt_mclock_WINCH_r > 0 )
+ {
+ cnt_mclock_WINCH_r -= 1;
+ }
+ else
+ {
+ cnt_mclock_WINCH_r = 0;
+ }
+ }
+ if( cnt_mclock_WINCH_r > motor_lock_chk_cnt )
+ {
+ flg_mclock_WINCH = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_WINCH = 0;
+ rts = false; // LOCK
+ }
+ }
+ }
+ // Check Tilt
+ else if( motor_number == MOTOR_CMPAN )
+ {
+ if( motor_dir == MOTOR_DIR_FWD )
+ {
+ mcchk = chk_motor_lock( mctr1_m1c.read()*100.0f, m_CMPAN_center_value, mc_th_CMPAN_f);
+ if( mcchk == true )
+ {
+ cnt_mclock_CMPAN_f += 1;
+ }
+ else
+ { if( cnt_mclock_CMPAN_f > 0 )
+ {
+ cnt_mclock_CMPAN_f -= 1;
+ }
+ else
+ {
+ cnt_mclock_CMPAN_f = 0;
+ }
+
+ }
+ if( cnt_mclock_CMPAN_f > motor_lock_chk_cnt )
+ {
+ flg_mclock_CMPAN = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_CMPAN = 0;
+ rts = false; // LOCK
+ }
+ }
+ else
+ {
+ mcchk = chk_motor_lock( mctr1_m1c.read()*100.0f, m_CMPAN_center_value, mc_th_CMPAN_r);
+ if( mcchk == true )
+ {
+ cnt_mclock_CMPAN_r += 1;
+ }
+ else
+ { if( cnt_mclock_CMPAN_r > 0 )
+ {
+ cnt_mclock_CMPAN_r -= 1;
+ }
+ else
+ {
+ cnt_mclock_CMPAN_r = 0;
+ }
+ }
+ if( cnt_mclock_CMPAN_r > motor_lock_chk_cnt )
+ {
+ flg_mclock_CMPAN = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_CMPAN = 0;
+ rts = false; // LOCK
+ }
+ }
+ }
+ // Check Tilt
+ else if( motor_number == MOTOR_CTILT )
+ {
+ if( motor_dir == MOTOR_DIR_FWD )
+ {
+ mcchk = chk_motor_lock( mctr1_m2c.read()*100.0f, m_CTILT_center_value, mc_th_CTILT_f);
+ if( mcchk == true )
+ {
+ cnt_mclock_CTILT_f += 1;
+ }
+ else
+ { if( cnt_mclock_CTILT_f > 0 )
+ {
+ cnt_mclock_CTILT_f -= 1;
+ }
+ else
+ {
+ cnt_mclock_CTILT_f = 0;
+ }
+ }
+ if( cnt_mclock_CTILT_f > motor_lock_chk_cnt )
+ {
+ flg_mclock_CTILT = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_CTILT = 0;
+ rts = false; // LOCK
+ }
+ }
+ else
+ {
+ mcchk = chk_motor_lock( mctr1_m2c.read()*100.0f, m_CTILT_center_value, mc_th_CTILT_r);
+ if( mcchk == true )
+ {
+ cnt_mclock_CTILT_r += 1;
+ }
+ else
+ { if( cnt_mclock_CTILT_r > 0 )
+ {
+ cnt_mclock_CTILT_r -= 1;
+ }
+ else
+ {
+ cnt_mclock_CTILT_r = 0;
+ }
+ }
+ if( cnt_mclock_CTILT_r > motor_lock_chk_cnt )
+ {
+ flg_mclock_CTILT = 1;
+ rts = true; // LOCK
+ }
+ else
+ {
+ flg_mclock_CTILT = 0;
+ rts = false; // LOCK
+ }
+ }
+ }
+ return rts;
+}
+
+bool mcchk::chk_motor_lock(
+ float motor_current, // motor current
+ float motor_center_value, // motor current center value
+ int mc_threshold // motor current threshold
+){
+// int32_t mc_abs_pct;
+
+ _cnt_now = motor_current;
+ _cnt_center = motor_center_value;
+ _cnt_th = mc_threshold;
+
+ mc_abs_pct = abs((int32_t)((motor_current - motor_center_value)*100.0f));
+ if( mc_abs_pct > mc_threshold )
+ {
+ return true; // Motor current is over threshold.
+ }
+ else
+ {
+ return false; // Motor current isn't over threshold.
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/mcchk.h Thu Jul 26 00:20:15 2018 +0000
@@ -0,0 +1,66 @@
+class mcchk
+{
+private:
+ float m_LBCRW_center_value;
+ float m_RFCRW_center_value;
+ float m_LBTFM_center_value;
+ float m_RFTFM_center_value;
+ float m_WINCH_center_value;
+ float m_CMPAN_center_value;
+ float m_CTILT_center_value;
+
+ int8_t flg_mclock_LBCRW; // bit0: forward, bit1: reverse
+ int8_t flg_mclock_RFCRW; // bit0: forward, bit1: reverse
+ int8_t flg_mclock_LBTFM; // bit0: forward, bit1: reverse
+ int8_t flg_mclock_RFTFM; // bit0: forward, bit1: reverse
+ int8_t flg_mclock_CMPAN; // bit0: forward, bit1: reverse
+ int8_t flg_mclock_CTILT; // bit0: forward, bit1: reverse
+ int8_t flg_mclock_WINCH; // bit0: forward, bit1: reverse
+
+public:
+
+ int8_t cnt_mclock_LBCRW_f; // motor lock counter
+ int8_t cnt_mclock_LBCRW_r; // motor lock counter
+ int8_t cnt_mclock_RFCRW_f; // motor lock counter
+ int8_t cnt_mclock_RFCRW_r; // motor lock counter
+ int8_t cnt_mclock_LBTFM_f; // motor lock counter
+ int8_t cnt_mclock_LBTFM_r; // motor lock counter
+ int8_t cnt_mclock_RFTFM_f; // motor lock counter
+ int8_t cnt_mclock_RFTFM_r; // motor lock counter
+ int8_t cnt_mclock_CMPAN_f; // motor lock counter
+ int8_t cnt_mclock_CMPAN_r; // motor lock counter
+ int8_t cnt_mclock_CTILT_f; // motor lock counter
+ int8_t cnt_mclock_CTILT_r; // motor lock counter
+ int8_t cnt_mclock_WINCH_f; // motor lock counter
+ int8_t cnt_mclock_WINCH_r; // motor lock counter
+
+
+ int16_t mc_th_LBCRW_f;
+ int16_t mc_th_LBCRW_r;
+ int16_t mc_th_RFCRW_f;
+ int16_t mc_th_RFCRW_r;
+ int16_t mc_th_LBTFM_f;
+ int16_t mc_th_LBTFM_r;
+ int16_t mc_th_RFTFM_f;
+ int16_t mc_th_RFTFM_r;
+ int16_t mc_th_WINCH_f;
+ int16_t mc_th_WINCH_r;
+ int16_t mc_th_CMPAN_f;
+ int16_t mc_th_CMPAN_r;
+ int16_t mc_th_CTILT_f;
+ int16_t mc_th_CTILT_r;
+
+ int32_t mc_abs_pct;
+
+ float _cnt_now;
+ float _cnt_center;
+ int _cnt_th;
+
+ bool motor_lock_flg;
+
+ mcchk();
+ void set_init_mc( int );
+ bool rdnchk_motorCurrent( int8_t, int8_t, int8_t );
+ bool chk_motor_lock( float, float, int );
+ float mcchk::rd_motorCurrent( int no );
+};