Branch for servo motor
Dependencies: MPU6050-DMP mbed ros_lib_kinetic
Fork of AGV_SMT by
Diff: main.cpp
- Revision:
- 3:51194773aa7e
- Parent:
- 2:648583d6e41a
- Child:
- 4:b728b0359ec8
diff -r 648583d6e41a -r 51194773aa7e main.cpp --- a/main.cpp Thu Apr 12 01:15:57 2018 +0000 +++ b/main.cpp Fri Apr 13 06:58:49 2018 +0000 @@ -1,5 +1,5 @@ /* - +0412 combine the sevro motor encoder to publish */ #include "MPU6050_6Axis_MotionApps20.h" #include "mbed.h" @@ -30,6 +30,8 @@ Serial RS485(PA_9, PA_10); DigitalOut RS485_E(D7); //RS485_E DigitalOut myled(LED1); +Ticker CheckDataR; + MPU6050 mpu;//(PB_7,PB_6); // sda, scl pin tiny_msgs::tinyIMU imu_msg; @@ -73,6 +75,15 @@ double rate; double Dimeter; float dx,dy,dr; + +int buffer[9] = {0}; + +//=========RS485 +char recChar=0; +bool recFlag=false; +char recArr[20]; +int index=0; + struct Offset { int16_t ax, ay, az; int16_t gx, gy, gz; @@ -92,10 +103,14 @@ void dmpDataUpdate(); unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen); void setAddress(char MotorAddress,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 TwistToMotors(); +void flushSerialBuffer(); +void readData(); //=================================================== bool Init() { @@ -201,12 +216,53 @@ } 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; +// char dataH,dataL; + int i; + + //MotorAddress = address; + Function = 0x03; + //DataAddress = data; + //Data = 0x0001; + DataAddressH = ((DataAddress>>8)&0xFF); + DataAddressL = ((DataAddress>>0)&0xFF); + + 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++) + { + RS485.printf("%c",sendData[i]); + } + + wait_ms(10); + RS485_E = 0; + //onInterrupt(); + //=========================================== +} void SVON(char MotorStation) { //void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data) - setAddress(MotorStation,0x0214,0x0100); + setAddress(MotorStation,0x0214,0x0001); } void SVOFF(char MotorStation) @@ -215,6 +271,12 @@ setAddress(MotorStation,0x0214,0x0101); wait_ms(10); } +void ReadENC(char MotorStation) +{ + ReadAddress(MotorStation,0x0066); + wait_ms(10); + +} int myabs( int a ){ if ( a < 0 ){ return -a; @@ -255,17 +317,10 @@ setAddress(Motor1,0x0112,Lrpm); setAddress(Motor2,0x0112,Rrpm); - + 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 ); - //} - //pc.printf("Wr = %.1f\n",vel_data[0]); - //pc.printf("Wl = %.1f\n",vel_data[1]); + ticks_since_target += 1; } @@ -277,6 +332,7 @@ dy = msg.linear.y; dr = msg.angular.z; TwistToMotors(); + ReadENC(Motor1); } ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb); //====================================================================================== @@ -358,19 +414,51 @@ } } +//================RS485======================================= +void flushSerialBuffer() { + + 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; + //===================== +} +//======================================================= + int main() { RS485.baud(PC_BAUDRATE); MBED_ASSERT(Init() == true); - +// myled = 0; nh.initNode(); nh.advertise(imu_pub); // nh.advertise(pub_lmotor); // nh.advertise(pub_rmotor); nh.subscribe(cmd_vel_sub); - SVON(1); - SVON(2); +// SVON(1); +// SVON(2); + flushSerialBuffer(); + RS485.attach(&readData);//,0.25); + //CheckDataR.attach(&CALL_chDataR,0.25); while(1) { +//================================== + +//============================= + seq++; mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); @@ -387,6 +475,9 @@ nh.spinOnce(); wait_ms(10); //writing current accelerometer and gyro position - //pc.printf("%d;%d;%d;%d;%d;%d\r\n",ax,ay,az,gx,gy,gz); + //pc.printf("%d;%d;%d;%d;%d;%d\r\n",ax,ay,az,gx,gy,gz); + +// RS485.attach(&onInterrupt,Serial::RxIrq); + } } \ No newline at end of file