z ysaito
2018.07.26
4_main.cpp
- Committer:
- sayzyas
- Date:
- 2018-07-26
- Revision:
- 0:b022386f217f
File content as of revision 0:b022386f217f:
/**********************************************************
* Project: B2 (1F-1)
* Title: CrExp B2 Motor Ctrl Main
* Target: LPCXpresso824-Max
* Author: zinsor
* Date(Latest update) 2015.12.21(Mon)
* --------------------------------------------------------
* Article
* Notification: Moving and Fix Winch rotation direction
* is opposit.
*
* --------------------------------------------------------
*
* 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 -> Resolver B
* +5V | ## ##### # | SSEL P0_15 D10 -> Resolver A
* GND | | P0_27 D9
* GND | | P0_13 D8
* N/A | | P0_17 D7
* | | P0_16 D6
* A0 P0_6 | | PWM P0_28 D5
* A1 P0_14 | | PWM P0_18 D4
* A2 P0_23 | | PWM P0_12 D3
* A3 P0_22 | | PWM P0_19 D2
* A4 P0_21 | | TX P0_4 D1
* A5 P0_20 | | RX P0_0 D0
* +---------------------+
*
***************************************/
#include "mbed.h"
//#include "rtos.h"
#include "QEI.h"
#include "common.h"
#include <math.h>
// Hardware setting
Serial pc(USBTX, USBRX); // tx, rx
#ifdef FFWinchPhaseSetting // For Fix Fallong Winch
QEI wheel( P0_26, P0_15, 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 );
#endif // FFWinchPhaseSetting
/*
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
I2CSlave slave(P0_11, P0_10); //I2C SDA SCL
int i2c_saddress = 0;
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);
}
// ========================================================================
// Thread: Resolver Control Task
// ========================================================================
void ResolverCtrl_task(void const *) {
}
// ========================================================================
// Thread: Main Controller Interface Task
// Get Command from Main controller and do the task.
// ========================================================================
void MainCtrl_Interface_task(void const *) {
char buf[14]; // command buffer
// char msg[] = "C000000";
// char res_msg[] = "Slave!";
char res_msg2[] = "Bravo!";
// int flg_readCurrent = 0;
int pulse;
int16_t distance;
int16_t position_offset = 0;;
uint16_t dram_diameter;
uint16_t ccable_diameter;
uint8_t rresolution;
int tmp;
slave.address(i2c_saddress);
DEBUG_PRINT_L1("*** Start Resolver thread ***\r\n");
/*
* I2C Access
*/
while(1) {
// DEBUG_PRINT_L1(".");
int i = slave.receive();
slave.read(buf, NumberOfI2CCommand);
// DEBUG_PRINT_L1("?");
// DEBUG_PRINT_L1("Slave Received\r\n");
switch (i) {
case I2CSlave::NoData:
// DEBUG_PRINT_L1("the slave has not been addressed\r\n");
// read_motor_current(2);
break;
//
// Slave(this) <== Master
//
case I2CSlave::ReadAddressed:
/* Master is reading winch current position here */
// DEBUG_PRINT_L1("the master has requested a read from this slave\r\n");
pulse = wheel.getPulses();
// distance = (int16_t)( pulse * ( (double)( (((double)dram_diameter+(double)ccable_diameter)/(double)100) * (double)PAI / (pow(2.0, (double)rresolution)*4 ) ) ) );
distance = (int16_t)( pulse * ( (double)( ((double)dram_diameter/(double)100) * (double)PAI / (pow(2.0, (double)rresolution)*4 ) ) ) );
// distance = (int16_t)((double)distance * (double)0.9986);
distance = (int16_t)((double)distance * (double)((double)ccable_diameter/(double)10000));
// -------------------------------
distance += position_offset; // 2016.11.17
// -------------------------------
// distance = (int16_t)( pulse * ( REAL_THREAD_DIAMETER * PAI / ROTATION_PULSE_PER_1ROUND ));
DEBUG_PRINT_L1("Bd4> PULSE: %07d, ", pulse);
DEBUG_PRINT_L1("DISTANCE(OFFSET): %04d(%4d) mm", distance, position_offset);
/*
2016.11.09
Add dummy data to read winch position value.
byte[0]: Dummy data = 0x12 <--- New added !
byte[1]: Winch position upper byte
byte[2]: Winch position lower byte
byte[3]: Dummy data = 0x34 <--- New added !
*/
res_msg2[0] = 0x12; // Dummy data
res_msg2[1] = distance & 0xFF;
res_msg2[2] = (distance >> 8)&0xFF;
res_msg2[3] = 0x34; // Dummy data
slave.write(res_msg2, 4); // Includes null char
tmp = (res_msg2[2] < 8)&0xFF00 | res_msg2[1]&0xFF;
DEBUG_PRINT_L1("\tSend data : %04d(%02x,%02x)\r\n", tmp, res_msg2[1], res_msg2[0]);
break;
case I2CSlave::WriteGeneral:
DEBUG_PRINT_L1("Bd4> the master is writing to all slave\r\n");
slave.read(buf, NumberOfI2CCommand);
DEBUG_PRINT_L1("Bd4> Read G: %s\r\n", buf);
break;
//
// Master ==> Slave(this)
//
case I2CSlave::WriteAddressed:
/* ******************************************* */
/* Add setting data set operation in this area */
/* ******************************************* */
if( buf[I2C_CP_COMMAND_R] == 'Z'){
wheel.reset();
position_offset = 0;
}
// New command 2016.11.17 for customer request
else if( buf[I2C_CP_COMMAND_R] == 'S'){
wheel.reset();
position_offset = 0;
for( int j = 0; j < NumberOfI2CCommand; j++)
pc.printf("%02x ", buf[j]);
pc.printf( "\r\n" );
position_offset = ( buf[I2C_CP_PRESET_CPOS_UPPER] << 8 );
position_offset |= buf[I2C_CP_PRESET_CPOS_LOWER];
pc.printf("POSITION OFFSET = %d\r\n", position_offset);
}
else if( buf[I2C_CP_COMMAND_R] == 'R'){
for( int j = 0; j < NumberOfI2CCommand; j++)
pc.printf("%02x ", buf[j]);
pc.printf( "\r\n" );
dram_diameter = ( buf[I2C_CP_WDRAM_DIA_UPPER] << 8 );
dram_diameter |= buf[I2C_CP_WDRAM_DIA_LOWER];
ccable_diameter = ( buf[I2C_CP_CCABLE_DIA_UPPER] << 8 );
ccable_diameter |= buf[I2C_CP_CCABLE_DIA_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", ccable_diameter);
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+ccable_diameter));
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+ccable_diameter)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) );
DEBUG_PRINT_L1("Bd4> ===========================================\r\n");
pulse = wheel.getPulses();
DEBUG_PRINT_L1("Bd4> PULSE: %07d, ", pulse);
// DEBUG_PRINT_L1("Bd4> >DISTANCE: %04d (mm)\n", (int)( pulse * ( REAL_THREAD_DIAMETER * PAI / ROTATION_PULSE_PER_1ROUND )));
DEBUG_PRINT_L1("DISTANCE: %04d (mm)\r\n", (int)( pulse * ( (double)( ((dram_diameter+ccable_diameter)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) ) ) );
DEBUG_PRINT_L1("Bd4> ---------------------------------------\r\n");
}
break;
}
//Thread::wait(1);
wait_ms(1);
}
}
// ========================================================================
// Main Function
// ========================================================================
int main() {
char buf[14]; // command buffer
char res_msg2[] = "Bravo!";
int pulse;
int16_t distance;
int16_t position_offset = 0;;
uint16_t dram_diameter;
uint16_t ccable_diameter;
uint8_t rresolution;
int tmp;
int32_t counter = 0;
pc.baud(115200);
i2c_saddress = Target_IIC_ADDR; // defined at header
DEBUG_PRINT_L0("> Board type: Resolver Counter controller(0x%02x)\r\n",i2c_saddress );
i2c_saddress = I2C_ADDRESS_RESOLVER;
slave.address(i2c_saddress);
led_demo();
DEBUG_PRINT_L0("\r\n");
DEBUG_PRINT_L0("Bd4> +--------------------------------------\r\n");
DEBUG_PRINT_L0("Bd4> | B2 CrExp Resolver Pulse Counter Main \r\n");
DEBUG_PRINT_L0("Bd4> | Letest update: %s\r\n", LatestUpDate);
DEBUG_PRINT_L0("Bd4> | Program Revision: %s\r\n", ProgramRevision);
DEBUG_PRINT_L0("Bd4> | Author: %s\r\n", Author);
DEBUG_PRINT_L0("Bd4> | Copyright(C) 2015 %s Allright Reserved\r\n", Company);
DEBUG_PRINT_L0("Bd4> +--------------------------------------\r\n");
//Thread resolverControlTask1(MainCtrl_Interface_task, NULL, osPriorityNormal, 128 * 4);
wheel.reset();
while(1){
counter++;
if( counter >= 50 ){
led3 = !led3; // Blue
counter = 0;
}
// DEBUG_PRINT_L1(".");
int i = slave.receive();
slave.read(buf, NumberOfI2CCommand);
// DEBUG_PRINT_L1("?");
// DEBUG_PRINT_L1("Slave Received\r\n");
switch (i) {
case I2CSlave::NoData:
// DEBUG_PRINT_L1("the slave has not been addressed\r\n");
// read_motor_current(2);
break;
//
// Slave(this) <== Master
//
case I2CSlave::ReadAddressed:
/* Master is reading winch current position here */
// DEBUG_PRINT_L1("the master has requested a read from this slave\r\n");
pulse = wheel.getPulses();
// distance = (int16_t)( pulse * ( (double)( (((double)dram_diameter+(double)ccable_diameter)/(double)100) * (double)PAI / (pow(2.0, (double)rresolution)*4 ) ) ) );
distance = (int16_t)( pulse * ( (double)( ((double)dram_diameter/(double)100) * (double)PAI / (pow(2.0, (double)rresolution)*4 ) ) ) );
// distance = (int16_t)((double)distance * (double)0.9986);
distance = (int16_t)((double)distance * (double)((double)ccable_diameter/(double)10000));
// -------------------------------
distance += position_offset; // 2016.11.17
// -------------------------------
// distance = (int16_t)( pulse * ( REAL_THREAD_DIAMETER * PAI / ROTATION_PULSE_PER_1ROUND ));
DEBUG_PRINT_L1("Bd4> PULSE: %07d, ", pulse);
DEBUG_PRINT_L1("DISTANCE(OFFSET): %04d(%4d) mm", distance, position_offset);
/*
2016.11.09
Add dummy data to read winch position value.
byte[0]: Dummy data = 0x12 <--- New added !
byte[1]: Winch position upper byte
byte[2]: Winch position lower byte
byte[3]: Dummy data = 0x34 <--- New added !
*/
res_msg2[0] = 0x12; // Dummy data
res_msg2[1] = distance & 0xFF;
res_msg2[2] = (distance >> 8)&0xFF;
res_msg2[3] = 0x34; // Dummy data
slave.write(res_msg2, 4); // Includes null char
tmp = (res_msg2[2] < 8)&0xFF00 | res_msg2[1]&0xFF;
DEBUG_PRINT_L1("\tSend data : %04d(%02x,%02x)\r\n", tmp, res_msg2[1], res_msg2[0]);
break;
case I2CSlave::WriteGeneral:
DEBUG_PRINT_L1("Bd4> the master is writing to all slave\r\n");
slave.read(buf, NumberOfI2CCommand);
DEBUG_PRINT_L1("Bd4> Read G: %s\r\n", buf);
break;
//
// Master ==> Slave(this)
//
case I2CSlave::WriteAddressed:
/* ******************************************* */
/* Add setting data set operation in this area */
/* ******************************************* */
if( buf[I2C_CP_COMMAND_R] == 'Z'){
wheel.reset();
position_offset = 0;
}
// New command 2016.11.17 for customer request
else if( buf[I2C_CP_COMMAND_R] == 'S'){
wheel.reset();
position_offset = 0;
for( int j = 0; j < NumberOfI2CCommand; j++)
pc.printf("%02x ", buf[j]);
pc.printf( "\r\n" );
position_offset = ( buf[I2C_CP_PRESET_CPOS_UPPER] << 8 );
position_offset |= buf[I2C_CP_PRESET_CPOS_LOWER];
pc.printf("POSITION OFFSET = %d\r\n", position_offset);
}
else if( buf[I2C_CP_COMMAND_R] == 'R'){
for( int j = 0; j < NumberOfI2CCommand; j++)
pc.printf("%02x ", buf[j]);
pc.printf( "\r\n" );
dram_diameter = ( buf[I2C_CP_WDRAM_DIA_UPPER] << 8 );
dram_diameter |= buf[I2C_CP_WDRAM_DIA_LOWER];
ccable_diameter = ( buf[I2C_CP_CCABLE_DIA_UPPER] << 8 );
ccable_diameter |= buf[I2C_CP_CCABLE_DIA_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", ccable_diameter);
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+ccable_diameter));
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+ccable_diameter)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) );
DEBUG_PRINT_L1("Bd4> ===========================================\r\n");
pulse = wheel.getPulses();
DEBUG_PRINT_L1("Bd4> PULSE: %07d, ", pulse);
// DEBUG_PRINT_L1("Bd4> >DISTANCE: %04d (mm)\n", (int)( pulse * ( REAL_THREAD_DIAMETER * PAI / ROTATION_PULSE_PER_1ROUND )));
DEBUG_PRINT_L1("DISTANCE: %04d (mm)\r\n", (int)( pulse * ( (double)( ((dram_diameter+ccable_diameter)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) ) ) );
DEBUG_PRINT_L1("Bd4> ---------------------------------------\r\n");
}
break;
}
//Thread::wait(1);
wait_ms(1);
}
}