Weber Yang
Branch for servo motor
Dependencies: MPU6050-DMP mbed ros_lib_kinetic
Fork of
AGV_SMT
by 
Weber Yang
Diff: main.cpp
- Revision:
- 6:eadfb1b45bda
- Parent:
- 5:52fb31c1a8c0
- Child:
- 7:1a234f02746f
--- a/main.cpp Tue Apr 17 07:59:09 2018 +0000
+++ b/main.cpp Thu May 10 01:39:34 2018 +0000
@@ -3,10 +3,10 @@
*/
#include "MPU6050_6Axis_MotionApps20.h"
#include "mbed.h"
-
-//#include "CANMsg.h"
+#include "CAN.h"
#include "I2Cdev.h"
#include "MPU6050.h"
+
#include <ros.h>
#include <ros/time.h>
#include <std_msgs/Empty.h>
@@ -36,10 +36,19 @@
#define DI1 0x0214 //0214H means digital input DI1 for sevro on
#define APR 0x0066 //0066H means encoder abs rev
#define SP1 0x0112
+
+#define CAN_DATA 0x470
+#define CAN_STATUS 0x471
+
//Serial pc(USBTX,USBRX);
-Serial rs485(PA_9, PA_10);
+Timer t;
+Serial RS232(PA_9, PA_10);
DigitalOut Receiver(D7); //RS485_E
-DigitalOut myled(LED1);
+DigitalOut CAN_T(D14);
+DigitalOut CAN_R(D15);
+//CAN can1(PB_8,PB_9); // CAN Rx pin name, CAN Tx pin name
+//CANMsg rxMsg;
+//CANMessage rxMsg;
Ticker CheckDataR;
MPU6050 mpu;//(PB_7,PB_6); // sda, scl pin
@@ -48,12 +57,13 @@
std_msgs::String str_msg;
std_msgs::Float32 VelAngular_L;
std_msgs::Float32 VelAngular_R;
+sensor_msgs::BatteryState BTState;
ros::NodeHandle nh;
ros::Publisher imu_pub("tinyImu", &imu_msg);
ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L);
ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R);
-
+ros::Publisher BT_pub("BatteryState", &BTState);
uint32_t seq;
#define IMU_FIFO_RATE_DIVIDER 0x09
@@ -61,7 +71,7 @@
#define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000
#define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2
-#define PC_BAUDRATE 38400
+#define PC_BAUDRATE 115200//38400
#define DEG_TO_RAD(x) ( x * 0.01745329 )
#define RAD_TO_DEG(x) ( x * 57.29578 )
@@ -94,6 +104,13 @@
char recArr[20];
int index=0;
+ uint32_t SOC;
+ uint32_t Tempert;
+ uint32_t RackVoltage = 0;
+ uint32_t Current = 0;
+ uint32_t MaxCellV = 0;
+ uint32_t MinCellV = 0;
+
struct Offset {
int16_t ax, ay, az;
int16_t gx, gy, gz;
@@ -112,15 +129,8 @@
bool Init();
void dmpDataUpdate();
unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen);
-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);
-void SVOFF(char MotorStation);
int myabs( int a );
void TwistToMotors();
-void flushSerialBuffer();
-void readData();
//===================================================
@@ -149,96 +159,133 @@
}
return wCrc;
}
-
-void setAddress(char MotorAddress,char func,int16_t DataAddress,int16_t Data)
+void Sendmessage(float Rrpm,float Lrpm)
{
- unsigned char sendData[16];
-// int16_t tmpCRC;
-// char DataAddressH,DataAddressL;
-// char dataH,dataL;
+ //RS232.printf("Wr = %.1f\n",Rrpm);
+ //RS232.printf("Wl = %.1f\n",Lrpm);
+unsigned char sendData[16];
+unsigned int tmpCRC;
+int motor1,motor2;
+
+ sendData[0] = Start;
+ sendData[1] = Address;
+ sendData[2] = ReturnType;
+ sendData[3] = Clean;
+ sendData[4] = Reserve;
+ sendData[5] = 0x01;//motor1Sevro ON
+ sendData[6] = 0x01;//motor2Sevro ON
+if (Rrpm>0){sendData[7] = 0x01;}else{sendData[7] = 0x00;}
+if (Lrpm>0){sendData[8] = 0x00;}else{sendData[8] = 0x01;}
+ motor1 = abs(Rrpm);
+ motor2 = abs(Lrpm);
+
+ sendData[9] = (motor1>>8);//motor1speedH
+ sendData[10] = (motor1 & 0xFF);//motor1speedL
+ sendData[11] = (motor2>>8);//motor2speedH
+ sendData[12] = (motor2 & 0xFF);//motor2speedL
+ sendData[13] = End;
+ tmpCRC = CRC_Verify(sendData, 14);
+ sendData[14] = (tmpCRC & 0xFF);
+ sendData[15] = (tmpCRC>>8);
int i;
- Receiver = 1;
- wait_ms(1);
- sendData[0] = MotorAddress;
- 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++)
+ for (i=0;i<16;i++)
{
- rs485.printf("%02X",sendData[i]);
+ RS232.printf("%c",sendData[i]);
}
- wait_ms(1);
- Receiver = 0;
- wait_ms(10); //give 10ms let receive complete.
-}
-
-void SVON(char MotorStation)
+ RS232.printf("\r\n");
+}
+void TwistToMotors()
{
- //void setAddress(char MotorAddress,char function, int16_t DataAddress,int16_t setData)
- setAddress(MotorStation,Write,DI1,0x0001);
-}
+ w = 0.302;//0.2 ;//m
+ rate = 20;//50;
+ timeout_ticks = 2;
+ Dimeter = 0.127;//0.15;
+ float right,left;
+ float motor_rpm_r, motor_rpm_l;
+ double vel_data[2];
+ right = ( 1.0 * dx ) + (dr * w /2);
+// left = ( 1.0 * dx ) - (dr * w /2);
+// vel_data[0] = right*rate/Dimeter/60*1000;
+// vel_data[1] = left*rate/Dimeter/60*1000;
+ if (dx!=0)
+ {
+ if (dx>0)
+ {
+ if (dr >=0)
+ {
+ motor_rpm_r=300+(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
+ motor_rpm_l=300;
+ }
+ else
+ {
+ motor_rpm_r=300;
+ motor_rpm_l=300-(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
+ }
+ }
+ else
+ {
+ if(dr>=0)
+ {
+ motor_rpm_r=(-300)-(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
+ motor_rpm_l=(-300);
+ }
+ else
+ {
+ motor_rpm_r=(-300);
+ motor_rpm_l=(-300)+(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
+ }
+ }
+ }
+ else
+ {
+ if(dr>=0)
+ {
+ motor_rpm_r=100;
+ motor_rpm_l=-100;
+ }
+ else
+ {
+ motor_rpm_r=-100;
+ motor_rpm_l=100;
+ }
+ }
+ vel_data[0]=motor_rpm_r;
+ vel_data[1]=motor_rpm_l;
+ //================================================================ Original Version
+ /*if (dr>=0)
+ {
+ right = ( 1.0 * dx ) + (dr * w /2);
+ left = ( 1.0 * dx );
+ }
+ else
+ {
+ right = ( 1.0 * dx );
+ left = ( 1.0 * dx ) - (dr * w /2);
+ }
+ vel_data[0] = right*rate/(Dimeter/2)/(2*3.1416)*60;
+ vel_data[1] = left*rate/(Dimeter/2)/(2*3.1416)*60;*/
+//===================================================================
+ Sendmessage(vel_data[0],vel_data[1]);
+ VelAngular_R.data = vel_data[0];
+ VelAngular_L.data = vel_data[1];
+ //if(VelAngular_R.data >2000 || VelAngular_L.data>2000){
+ //}
+ //else{
+ pub_rmotor.publish( &VelAngular_R );
+ pub_lmotor.publish( &VelAngular_L );
+ //}
+ //RS232.printf("Wr = %.1f\n",vel_data[0]);
+ //RS232.printf("Wl = %.1f\n",vel_data[1]);
+ ticks_since_target += 1;
-void SVOFF(char MotorStation)
-{
- //void setAddress(char MotorAddress,char function, int16_t DataAddress,int16_t setData)
- setAddress(MotorStation,Write,DI1,0x0101);
- wait_ms(10);
-}
-void ReadENC(char MotorStation)
-{
- //void setAddress(char MotorAddress,char function, int16_t DataAddress,int16_t readDataNum)
- setAddress(MotorStation,Read,APR,0x0001);
- wait_ms(10);
-
}
+
int myabs( int a ){
if ( a < 0 ){
return -a;
}
return a;
}
-void TwistToMotors()
-{
- seq = seq + 1;
- //int lower_bound = 100;
- //int upper_bound = 300;
- float right,left;
- float motor_rpm_r, motor_rpm_l;
- //double vel_data[2];
- int16_t Lrpm,Rrpm;
- float vel_data[2];
- w = 0.302;//0.2 ;//m
- rate = 20;//50;
- timeout_ticks = 2;
- Dimeter = 0.127;//0.15;
-
- // prevent agv receive weird 1.0 command from cmd_vel
- if (dr == 1.0){
-
- dr = 0.001;
- }
-
- //
- right = ( 1.0 * dx ) + (dr * w /2);
- left = ( 1.0 * dx ) - (dr * w /2);
- motor_rpm_r = right*rate/(Dimeter/2)*60/(2*3.1416);
- motor_rpm_l = left*rate/(Dimeter/2)*60/(2*3.1416);
- vel_data[0] = motor_rpm_r;
- vel_data[1] = motor_rpm_l;
-
- Lrpm = motor_rpm_l;
- Rrpm = motor_rpm_r;
-
-// setAddress(Motor1,Write,0x0112,Lrpm);
-// setAddress(Motor2,Write,0x0112,Rrpm);
-
- ticks_since_target += 1;
-}
int str2int(const char* str, int star, int end)
{
@@ -250,7 +297,7 @@
}
return ret;
}
-
+//======================================================================================
void messageCb(const geometry_msgs::Twist &msg)
{
ticks_since_target = 0;
@@ -262,7 +309,6 @@
}
ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb);
//======================================================================================
- //
void dmpDataUpdate() {
// Check that this interrupt has enabled.
if (dmpReady == false) return;
@@ -339,59 +385,6 @@
// pc.printf("\n");
}
}
-
-//================rs485=======================================
-void flushSerialBuffer() {
-
- while (rs485.readable()) {
- rs485.getc();
- }
-}
-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;
- }
- }
-}
//=======================================================
bool Init() {
seq = 0;
@@ -399,31 +392,32 @@
nh.advertise(imu_pub);
nh.advertise(pub_lmotor);
nh.advertise(pub_rmotor);
+ nh.advertise(BT_pub);
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.
+ //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");
@@ -431,34 +425,61 @@
}
//=======================================================
int main() {
- rs485.baud(PC_BAUDRATE);
+ RS232.baud(PC_BAUDRATE);
MBED_ASSERT(Init() == true);
-
- SVON(1);
- SVON(2);
- flushSerialBuffer();
- rs485.attach(&readData);//,0.25);
- while(1)
- {
+ CANMessage rxMsg;
+ CAN_T = 0;
+ CAN_R = 0;
+ wait_ms(50);
+ CAN can1(D15,D14);//PB_8,PB_9); // CAN Rx pin name, CAN Tx pin name
+ wait_ms(50);
+ can1.frequency(500000);
+ wait_ms(50);
+ while(1){
seq++;
- mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+ t.start();
+// mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
- imu_msg.header.stamp = nh.now();
- imu_msg.header.frame_id = 0;
- imu_msg.header.seq = seq;
- imu_msg.accel.x = ax;
- imu_msg.accel.y = ay;
- imu_msg.accel.z = az;
- imu_msg.gyro.x = gx;
- imu_msg.gyro.y = gy;
- imu_msg.gyro.z = gz;
- imu_pub.publish( &imu_msg );
+ // imu_msg.header.stamp = nh.now();
+// imu_msg.header.frame_id = 0;
+// imu_msg.header.seq = seq;
+// imu_msg.accel.x = ax;
+// imu_msg.accel.y = ay;
+// imu_msg.accel.z = az;
+// imu_msg.gyro.x = gx;
+// imu_msg.gyro.y = gy;
+// imu_msg.gyro.z = gz;
+//
+// imu_pub.publish( &imu_msg );
+ if (can1.read(rxMsg) && (t.read_ms() > 5000)) {
+// RS232.printf(" ID = 0x%.3x\r\n", rxMsg.id);
+
+ if(rxMsg.id == CAN_DATA){
+ SOC = rxMsg.data[0]>>1;
+ Tempert = rxMsg.data[1]-50;
+ RackVoltage = ((unsigned int)rxMsg.data[3] << 8) + rxMsg.data[2];
+ Current = ((unsigned int)rxMsg.data[5] << 8) + rxMsg.data[4];
+ MaxCellV = rxMsg.data[6];
+ MinCellV = rxMsg.data[7];
+// RS232.printf("SOC = %d\r\n",SOC);
+// RS232.printf("Tempert = %d\r\n",Tempert);
+// RS232.printf("RackVoltage = %.2f\r\n",RackVoltage*0.1);
+// RS232.printf("Current = %.2f\r\n",Current*0.1);
+// RS232.printf("MaxCellV = %.2f\r\n",MaxCellV*0.02);
+// RS232.printf("MinCellV = %.2f\r\n",MinCellV*0.02);
+
+ BTState.header.stamp = nh.now();
+ BTState.header.frame_id = 0;
+ BTState.header.seq = seq;
+ BTState.voltage = RackVoltage*0.1;
+ BTState.current = Current;
+ BTState.design_capacity = 80;
+ BTState.percentage = SOC;
+ BT_pub.publish( &BTState );
+ t.reset();
+ } // if
+ } // if
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);
-
+ wait_ms(20);
}
}
\ No newline at end of file