![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Branch for servo motor
Dependencies: MPU6050-DMP mbed ros_lib_kinetic
Fork of AGV_SMT by
Diff: main.cpp
- Revision:
- 5:52fb31c1a8c0
- Parent:
- 4:b728b0359ec8
- Child:
- 6:eadfb1b45bda
diff -r b728b0359ec8 -r 52fb31c1a8c0 main.cpp --- a/main.cpp Fri Apr 13 07:03:10 2018 +0000 +++ b/main.cpp Tue Apr 17 07:59:09 2018 +0000 @@ -3,6 +3,7 @@ */ #include "MPU6050_6Axis_MotionApps20.h" #include "mbed.h" + //#include "CANMsg.h" #include "I2Cdev.h" #include "MPU6050.h" @@ -17,6 +18,8 @@ #include <stdio.h> #include <tiny_msgs/tinyVector.h> #include <tiny_msgs/tinyIMU.h> +#include <string> +#include <cstdlib> #define Start 0xAA #define Address 0x7F @@ -26,9 +29,16 @@ #define End 0x55 #define Motor1 1 #define Motor2 2 -//Serial pc(PA_2, PA_3,9600); -Serial RS485(PA_9, PA_10); -DigitalOut RS485_E(D7); //RS485_E +#define LENG 31 //0x42 + 31 bytes equal to 32 bytes + +#define Write 0x06 +#define Read 0x03 +#define DI1 0x0214 //0214H means digital input DI1 for sevro on +#define APR 0x0066 //0066H means encoder abs rev +#define SP1 0x0112 +//Serial pc(USBTX,USBRX); +Serial rs485(PA_9, PA_10); +DigitalOut Receiver(D7); //RS485_E DigitalOut myled(LED1); Ticker CheckDataR; @@ -51,7 +61,7 @@ #define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000 #define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2 -#define PC_BAUDRATE 115200 +#define PC_BAUDRATE 38400 #define DEG_TO_RAD(x) ( x * 0.01745329 ) #define RAD_TO_DEG(x) ( x * 57.29578 ) @@ -77,7 +87,7 @@ float dx,dy,dr; int buffer[9] = {0}; - +int dataH,datanum; //=========RS485 char recChar=0; bool recFlag=false; @@ -102,7 +112,7 @@ bool Init(); void dmpDataUpdate(); unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen); -void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data); +void setAddress(char MotorAddress,char func,int16_t DataAddress,int16_t Data); void ReadAddress(char MotorAddress,int16_t DataAddress); void SVON(char MotorStation); void ReadENC(char MotorStation); @@ -113,44 +123,7 @@ void readData(); //=================================================== -bool Init() { - //pc.baud(PC_BAUDRATE); -// pc.printf("Init.\n"); - seq = 0; - //while(!pc.readable()); - // pc.getc(); - - nh.initNode(); - mpu.initialize(); - if (mpu.testConnection()) { -// pc.printf("MPU6050 test connection passed.\n"); - } else { -// pc.printf("MPU6050 test connection failed.\n"); - return false; - } - if (mpu.dmpInitialize() == 0) { -// pc.printf("succeed in MPU6050 DMP Initializing.\n"); - } else { -// pc.printf("failed in MPU6050 DMP Initializing.\n"); - return false; - } - mpu.setXAccelOffset(offset.ax); - mpu.setYAccelOffset(offset.ay); - mpu.setZAccelOffset(offset.az); - mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000); - mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2); - mpu.setXGyroOffsetUser(offset.gx); - mpu.setYGyroOffsetUser(offset.gy); - mpu.setZGyroOffsetUser(offset.gz); - mpu.setDMPEnabled(true); // Enable DMP - packetSize = mpu.dmpGetFIFOPacketSize(); - dmpReady = true; // Enable interrupt. - - //pc.printf("Init finish!\n"); - - return true; -} //======================= motor ================================================= unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen) { @@ -177,103 +150,49 @@ return wCrc; } -void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data) +void setAddress(char MotorAddress,char func,int16_t DataAddress,int16_t Data) { - unsigned char sendData[8]; - int16_t tmpCRC; - //char MotorAddress; - char Function; - //int DataAddress,Data; - char DataAddressH,DataAddressL; - char dataH,dataL; - int i; - sendData[6] = 0xff; - sendData[7] = 0xff; - - //MotorAddress = address; - Function = 0x06; - //DataAddress = data; - //Data = 0x0001; - DataAddressH = ((DataAddress>>8)&0xFF); - DataAddressL = ((DataAddress>>0)&0xFF); - dataH = ((Data>>8)&0xFF); - dataL = ((Data>>0)&0xFF); - - sendData[0] = MotorAddress; - sendData[1] = Function; - sendData[2] = DataAddressH; - sendData[3] = DataAddressL; - sendData[4] = dataH; - sendData[5] = dataL; - tmpCRC = CRC_Verify(sendData, 6); - sendData[6] = (tmpCRC & 0xFF); - sendData[7] = (tmpCRC>>8); - RS485_E = 1; - wait_ms(10); - for (i=0;i<8;i++) - { - RS485.printf("%c",sendData[i]); - } - wait_ms(10); - RS485_E = 0; - //RS485.attach(&onInterrupt,Serial::RxIrq); - //=========================================== -} - -void ReadAddress(char MotorAddress,int16_t DataAddress) -{ - unsigned char sendData[8]; - int16_t tmpCRC; - //char MotorAddress; - char Function; - //int DataAddress,Data; - char DataAddressH,DataAddressL; + unsigned char sendData[16]; +// int16_t tmpCRC; +// char DataAddressH,DataAddressL; // char dataH,dataL; int i; - - //MotorAddress = address; - Function = 0x03; - //DataAddress = data; - //Data = 0x0001; - DataAddressH = ((DataAddress>>8)&0xFF); - DataAddressL = ((DataAddress>>0)&0xFF); - + Receiver = 1; + wait_ms(1); sendData[0] = MotorAddress; - sendData[1] = Function; - sendData[2] = DataAddressH; - sendData[3] = DataAddressL; - sendData[4] = 0x00;//dataH; - sendData[5] = 0x02;//dataL; - tmpCRC = CRC_Verify(sendData, 6); - sendData[6] = (tmpCRC & 0xFF); - sendData[7] = (tmpCRC>>8); - RS485_E = 1; - wait_ms(10); - for (i=0;i<8;i++) + sendData[1] = func; + sendData[2] = ((DataAddress>>8)&0xFF); + sendData[3] = ((DataAddress>>0)&0xFF); + sendData[4] = ((Data>>8)&0xFF); + sendData[5] = ((Data>>0)&0xFF); + sendData[6] = sendData[0]+sendData[1]+sendData[2]+sendData[3]+sendData[4]+sendData[5]; + sendData[6] = (~sendData[6]+1); + rs485.printf(":"); + for (i=0;i<7;i++) { - RS485.printf("%c",sendData[i]); + rs485.printf("%02X",sendData[i]); } - - wait_ms(10); - RS485_E = 0; - //onInterrupt(); - //=========================================== -} + wait_ms(1); + Receiver = 0; + wait_ms(10); //give 10ms let receive complete. +} + void SVON(char MotorStation) { - //void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data) - setAddress(MotorStation,0x0214,0x0001); + //void setAddress(char MotorAddress,char function, int16_t DataAddress,int16_t setData) + setAddress(MotorStation,Write,DI1,0x0001); } void SVOFF(char MotorStation) { - //void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data) - setAddress(MotorStation,0x0214,0x0101); + //void setAddress(char MotorAddress,char function, int16_t DataAddress,int16_t setData) + setAddress(MotorStation,Write,DI1,0x0101); wait_ms(10); } void ReadENC(char MotorStation) { - ReadAddress(MotorStation,0x0066); + //void setAddress(char MotorAddress,char function, int16_t DataAddress,int16_t readDataNum) + setAddress(MotorStation,Read,APR,0x0001); wait_ms(10); } @@ -315,15 +234,22 @@ Lrpm = motor_rpm_l; Rrpm = motor_rpm_r; - setAddress(Motor1,0x0112,Lrpm); - setAddress(Motor2,0x0112,Rrpm); - - VelAngular_R.data = vel_data[0]; - VelAngular_L.data = vel_data[1]; +// setAddress(Motor1,Write,0x0112,Lrpm); +// setAddress(Motor2,Write,0x0112,Rrpm); ticks_since_target += 1; } - + +int str2int(const char* str, int star, int end) +{ + int i; + int ret = 0; + for (i = star; i < end+1; i++) + { + ret = ret *10 + (str[i] - '0'); + } + return ret; +} void messageCb(const geometry_msgs::Twist &msg) { @@ -332,7 +258,7 @@ dy = msg.linear.y; dr = msg.angular.z; TwistToMotors(); - ReadENC(Motor1); + //ReadENC(Motor1); } ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb); //====================================================================================== @@ -414,45 +340,104 @@ } } -//================RS485======================================= +//================rs485======================================= void flushSerialBuffer() { - while (RS485.readable()) { - RS485.getc(); + while (rs485.readable()) { + rs485.getc(); } } - -void readData() { - //RS485_E = 1; //1=write - //wait_ms(3); - //RS485.putc('C'); - //wait_ms(1); - - RS485_E = 0; - wait_ms(3); - flushSerialBuffer(); - wait_ms(1); - RS485_E = 1; - wait_ms(3); - myled = !myled; - //===================== +void readData() +{ + while (rs485.readable()) + { + recChar = rs485.getc(); + recArr[index] = recChar; + index++; + if ((recChar == '\r') || (index>18)) + { + datanum = str2int(recArr,1,2); + if (datanum == 1) + { + dataH = str2int(recArr,7,10); + //pc.printf("M1 = %d\r\n",dataH); + VelAngular_R.data = dataH; + } + else + { +// pc.printf("M1 = no data\r\n"); + VelAngular_R.data = 0; + } + + if (datanum == 2) + { + dataH = str2int(recArr,7,10); + VelAngular_L.data = dataH; + } + else + { +// pc.printf("M2 = no data\r\n"); + VelAngular_L.data = 0; + } + + pub_rmotor.publish( &VelAngular_R); + pub_lmotor.publish( &VelAngular_L); + + recArr[index] = 0; + index = 0; + //pc.printf("%s\r\n", recArr); + + flushSerialBuffer(); + recFlag = true; + } + } } //======================================================= - -int main() { - RS485.baud(PC_BAUDRATE); - MBED_ASSERT(Init() == true); -// myled = 0; +bool Init() { + seq = 0; nh.initNode(); nh.advertise(imu_pub); -// nh.advertise(pub_lmotor); -// nh.advertise(pub_rmotor); + nh.advertise(pub_lmotor); + nh.advertise(pub_rmotor); nh.subscribe(cmd_vel_sub); + mpu.initialize(); + if (mpu.testConnection()) { +// pc.printf("MPU6050 test connection passed.\n"); + } else { +// pc.printf("MPU6050 test connection failed.\n"); + return false; + } + if (mpu.dmpInitialize() == 0) { +// pc.printf("succeed in MPU6050 DMP Initializing.\n"); + } else { +// pc.printf("failed in MPU6050 DMP Initializing.\n"); + return false; + } + mpu.setXAccelOffset(offset.ax); + mpu.setYAccelOffset(offset.ay); + mpu.setZAccelOffset(offset.az); + mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000); + mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2); + mpu.setXGyroOffsetUser(offset.gx); + mpu.setYGyroOffsetUser(offset.gy); + mpu.setZGyroOffsetUser(offset.gz); + mpu.setDMPEnabled(true); // Enable DMP + packetSize = mpu.dmpGetFIFOPacketSize(); + dmpReady = true; // Enable interrupt. + + //pc.printf("Init finish!\n"); + + return true; +} +//======================================================= +int main() { + rs485.baud(PC_BAUDRATE); + MBED_ASSERT(Init() == true); + SVON(1); SVON(2); flushSerialBuffer(); - RS485.attach(&readData);//,0.25); - //CheckDataR.attach(&CALL_chDataR,0.25); + rs485.attach(&readData);//,0.25); while(1) { seq++; @@ -469,6 +454,10 @@ imu_msg.gyro.z = gz; imu_pub.publish( &imu_msg ); nh.spinOnce(); + setAddress(Motor1,Read,APR,0x0001); + setAddress(Motor2,Read,APR,0x0001); + setAddress(Motor1,Write,SP1,Lrpm); + setAddress(Motor2,Write,SP1,Rrpm); wait_ms(10); }