z ysaito
https://os.mbed.com/users/sayzyas
Dependencies: QEI TextLCD mbed
P1_1Y_main.cpp
- Committer:
- sayzyas
- Date:
- 2018-07-26
- Revision:
- 0:73dd48be5ca6
File content as of revision 0:73dd48be5ca6:
/**********************************************************
* 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);
}
}