Betago
v1
Dependencies: BEAR_Protocol_Edited BufferedSerial Debug MaxSonar PID Process QEI UI iSerial mbed
Fork of
CleaningMachine_Betago
by 
palm and chin
Diff: main.cpp
- Revision:
- 2:f873deba2305
- Parent:
- 1:45f1573d65a1
- Child:
- 3:edaab92dbd2f
--- a/main.cpp Mon Mar 21 20:21:12 2016 +0000
+++ b/main.cpp Tue May 24 10:25:10 2016 +0000
@@ -9,7 +9,9 @@
#include "Motion_EEPROM_Address.h"
#include "move.h"
#include "UNTRASONIC.h"
-
+#include "BufferedSerial.h"
+#include "rplidar.h"
+RPLidar lidar;
//#include "pidcontrol.h"
#define EEPROM_DELAY 2
@@ -23,31 +25,13 @@
DigitalIn encoderB_1(PB_2);
InterruptIn encoderA_2(PB_14);
DigitalIn encoderB_2(PB_15);
-DigitalOut rs485_dirc1(RS485_DIRC);
Timer timerStart;
Timeout time_getsensor;
Timeout time_distance;
Timeout shutdown;
move m1;
//*****************************************************/
-//--PID parameter--
-//-Upper-//
-float U_Kc=0.2;
-float U_Ki_gain=0.0;
-float U_Kd_gain=0.0;
-float U_Ti=0.0;
-float U_Td=0.0;
-float U_Ki=U_Kc*U_Ti;
-float U_Kd=U_Kc*U_Td;
-//-lower-//
-float L_Kc=0.2;
-float L_Ki_gain=0.0;
-float L_Kd_gain=0.0;
-float L_Ti=0.0;
-float L_Td=0.0;
-float L_Ki=L_Kc*L_Ti;
-float L_Kd=L_Kc*L_Td;
-//*****************************************************/
+
// Global //
//timer
int timer_now=0,timer_later=0;
@@ -61,39 +45,32 @@
double setp1=0,setp2=0;
float outPID =0;
-float VRmax,VLmax,VR,VL,KP_LEFT,KI_LEFT,KD_LEFT,KP_RIGHT,KI_RIGHT ,KD_RIGHT ;
+float VRmax=0,VLmax=0,VR=0,VL=0,KP_LEFT=0,KI_LEFT=0,KD_LEFT=0,KP_RIGHT=0,KI_RIGHT=0 ,KD_RIGHT=0 ;
PID P1(KP_LEFT,KI_LEFT,KD_LEFT,0.1);
PID P2(KP_RIGHT,KI_RIGHT ,KD_RIGHT,0.1);
//Ticker Recieve;
//-- Communication --
COMMUNICATION *com1;
-Serial PC(SERIAL_TX,SERIAL_RX);
+BufferedSerial PC(SERIAL_TX,SERIAL_RX);
+//Serial PC(SERIAL_TX,SERIAL_RX);
Bear_Receiver com(SERIAL_TX,SERIAL_RX,115200);
int16_t MY_ID = 0x00;
//-- Memorry --
EEPROM memory(PB_4,PA_8,0);
+float KP_LEFT_BUFF=0,KI_LEFT_BUFF=0,KD_LEFT_BUFF=0,KP_RIGHT_BUFF=0,KI_RIGHT_BUFF =0,KD_RIGHT_BUFF=0;
-//-- encoder --
+ void CmdCheck(int16_t id,uint8_t *command,uint8_t ins);
+ void RC();
-//-- Motor --
-//int dir;
-//Motor Upper(PWM_LU,A_LU,B_LU);
-//Motor Lower(PWM_LL,A_LL,B_LL);
-//-- PID --
-//int Upper_SetPoint=20;
-//int Lower_SetPoint=8;
-//PID Up_PID(U_Kc, U_Ti, U_Td, 0.001);//Kp,Ki,Kd,Rate
-//PID Low_PID(L_Kc, L_Ti, L_Td, 0.001);
+//rplidar
+ float distances = 0;
+ float angle = 0;
+bool startBit = 0;
+char quality =0 ;
-//PID Up_PID(U_Kc, U_Ti, U_Td);//Kp,Ki,Kd,Rate
-//PID Low_PID(L_Kc, L_Ti, L_Td);
-//*****************************************************/
-//void Start_Up();
void CmdCheck(int16_t id,uint8_t *command,uint8_t ins);
-//Timer counterUP;
-//Timer counterLOW;
DigitalOut myled(LED1);
@@ -139,19 +116,19 @@
Z_d+=1;
pulse_d=0;
}
- // pc.printf("%d",Encoderpos);
+
}
void getvelo1()//จาก encoder
{
valocity1=pulse_1*((2*3.14*r)/128);
- pc.printf("valocity=%f \n",valocity1);
+ PC.printf("valocity=%f \n",valocity1);
count=0;
timerStart.reset();
}
void getvelo2()
{
valocity2=pulse_2*((2*3.14*r)/128);
- pc.printf("valocity=%f \n",valocity2);
+ PC.printf("valocity=%f \n",valocity2);
count=0;
timerStart.reset();
}
@@ -161,6 +138,28 @@
//ส่งข้อมูล
}
+void get_rplidar()
+{
+ if (IS_OK(lidar.waitPoint())) {
+ distances = lidar.getCurrentPoint().distance; //distance value in mm unit
+ angle = lidar.getCurrentPoint().angle; //anglue value in degree
+ startBit = lidar.getCurrentPoint().startBit; //whether this point is belong to a new scan
+ quality = lidar.getCurrentPoint().quality; //quality of the current measurement
+ PC.printf("Distance = %.2f cm\n",distances/10);
+ wait_ms(100);
+ } else {
+ // analogWrite(RPLIDAR_MOTOR, 0); //stop the rplidar motor
+// rplidar_response_device_info_t info;
+ // if (IS_OK(lidar.getDeviceInfo(info, 100))) {
+ lidar.startScan();
+ // motor=1;
+ // start motor rotating at max allowed speed
+ // analogWrite(RPLIDAR_MOTOR, 255);
+ //delay(1000);
+ // }
+ }
+
+}
double map(double x, double in_min, double in_max, double out_min, double out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
@@ -198,141 +197,11 @@
P2.setProcessValue(valocity2);
outPID=P2.compute();
//pc.printf("outPID=%f \n",outPID);
- m1.movespeed_2(1,setp2,outPID);
-}
-/*
-void Read_Encoder(PinName Encoder)
-{
- ENC.format(8,0);
- ENC.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k
-
- if(Encoder == EncoderA) {
- EncA = 0;
- } else {
- EncB = 0;
- }
- ENC.write(0x41);
- ENC.write(0x09);
- data = ENC.write(0x00);
- if(Encoder == EncoderA) {
- EncA = 1;
- } else {
- EncB = 1;
- }
-
-}
-//****************************************************
-int Get_EnValue(int Val)
-{
- int i = 0;
- static unsigned char codes[] = {
- 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175,
- 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215,
- 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235,
- 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245,
- 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250,
- 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125,
- 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190,
- 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95
- };
- if ( MY_ID == 0x01 ) { //when it was left side
- while(Val != codes[i]) {
- i++;
- }
- }
- if ( MY_ID == 0x02 ) { //when it was right side
- while(Val != codes[127-i]) {
- i++;
- }
- }
- return i;
-
+ m1.movespeed_2(setp2,outPID);
}
-//****************************************************
-void SET_UpperPID()
-{
- Upper.period(0.001);
-
- memory.read(ADDRESS_UP_MARGIN,UpMargin);
- Up_PID.setMargin(UpMargin);
-
- memory.read(ADDRESS_UPPER_KP,U_Kc);
- Up_PID.setKp(U_Kc);
- memory.read(ADDRESS_UPPER_KI,U_Ki_gain);
- Up_PID.setKi(U_Ki_gain);
- memory.read(ADDRESS_UPPER_KD,U_Kd_gain);
- Up_PID.setKd(U_Kd_gain);
-
- //Up_PID.setMode(0);
- //Up_PID.setInputLimits(18,62);
- //Up_PID.setOutputLimits(0,1);
-}
-//******************************************************/
-/*
-void SET_LowerPID()
-{
- Lower.period(0.001);
-
- memory.read(ADDRESS_LOW_MARGIN,LowMargin);
- Low_PID.setMargin(LowMargin);
-
- memory.read(ADDRESS_LOWER_KP,L_Kc);
- Low_PID.setKp(L_Kc);
- memory.read(ADDRESS_LOWER_KI,L_Ki_gain);
- Low_PID.setKi(L_Ki_gain);
-
- memory.read(ADDRESS_LOWER_KD,L_Kd_gain);
- Low_PID.setKd(L_Kd_gain);
-
- //Low_PID.setMode(0);
- //Low_PID.setInputLimits(0,127); // set range
- //Low_PID.setOutputLimits(0,1);
-}
-//******************************************************/
-/*
-void Move_Upper()
-{
- Read_Encoder(EncoderA);
- Upper_Position = (float)Get_EnValue(data);
-#ifdef DEBUG_UP
- printf("read_encode = 0x%2x \n\r",data);
- printf("Setpoint = %d\n\r",Upper_SetPoint);
- printf("Upper_Position = %f\n\r",Upper_Position);
-#endif
- Up_PID.setCurrent(Upper_Position);
- float speed =Up_PID.compute();
-#ifdef DEBUG_UP
- printf("E_n= %f\n\r",Up_PID.getErrorNow());
- printf("Kp = %f\n\r",Up_PID.getKp());
- printf("speed = %f\n\n\n\r",speed);
-#endif
- Upper.speed(speed);
-}
-//******************************************************/
-/*
-void Move_Lower()
-{
- Read_Encoder(EncoderB);
- Lower_Position = (float)Get_EnValue(data);
- Low_PID.setCurrent(Lower_Position);
-#ifdef DEBUG_LOW
- printf("read_encode = 0x%2x \n\r",data);
- printf("Setpoint = %d\n\r",Lower_SetPoint);
- printf("Upper_Position = %f\n\r",Lower_Position);
-#endif
-
- float speed =Low_PID.compute();
-#ifdef DEBUG_LOW
- printf("E_n= %f\n\r",Low_PID.getErrorNow());
- printf("Kp = %f\n\r",Low_PID.getKp());
- printf("speed = %f\n\n\n\r",speed);
-#endif
- Lower.speed(speed);
-}
-//******************************************************/
-void Rc()
+void RC()
{
myled =1;
uint8_t data_array[30];
@@ -354,89 +223,22 @@
int main()
{
PC.baud(115200);
+ lidar.begin();
printf("******************");
- /*
- while(1)
- {
- Read_Encoder(EncoderA);
- Upper_Position = Get_EnValue(data);
- Read_Encoder(EncoderB);
- Lower_Position = Get_EnValue(data);
- PC.printf("Upper Position : %f\n",Upper_Position);
- PC.printf("Lower_Position : %f\n",Lower_Position);
- wait(0.5);
- }
- */
-
-
- //Recieve.attach(&Rc,0.025);
- //Start_Up();
-
- //SET_UpperPID();
- // SET_LowerPID();
-
- // printf("BEAR MOTION ID:0x%02x\n\r",MY_ID);
- /*
- while(1)
- {
-
- Upper.speed(-1);
- wait_ms(400);
- Upper.speed(1);
- wait_ms(400);
- Upper.break();
-
- Lower.speed(-1.0);
- wait_ms(401);
- Lower.speed(1.0);
- wait_ms(401);
- Lower.break();
- }
- */
-
- // counterUP.start();
-// counterLOW.start();
-
+
+
+
+ encoderA_1.rise(&EncoderA_1);
+ timerStart.start();
+ P1.setMode(1);
+ P1.setBias(0);
+ // pc.printf("READY \n");
+ //pc.attach(&Rx_interrupt, Serial::RxIrq);
while(1) {
- /*
- //printf("timer = %d\n\r",counter.read_ms());
- if(counterUP.read_ms() > 1400) {
-
- if(Upper_SetPoint < 53) {
- Upper_SetPoint++;
- } else
- Upper_SetPoint =18;
-
- counterUP.reset();
-
- }
-
- if(counterLOW.read_ms() > 700) {
-
- if(Lower_SetPoint < 100) {
- Lower_SetPoint++;
- } else
- Lower_SetPoint =8;
-
- counterLOW.reset();
-
- }
- */
- // myled =1;
- //wait_ms(10);
-///////////////////////////////////////////////////// start
- //Set Set_point
- // Up_PID.setGoal(Upper_SetPoint);
- // Low_PID.setGoal(Lower_SetPoint);
-
- //Control Motor
- // Move_Upper();
- // Move_Lower();
-///////////////////////////////////////////////////// stop =306us
- //uint8_t aaa[1]={10};
- //com.sendBodyWidth(0x01,aaa);
- Rc();
+
+ get_rplidar();
+ RC();
//wait_ms(1);
}
}
@@ -507,6 +309,7 @@
PC.printf("*****************************");
break;
}
+ //save to rom
case SET_KP_LEFT: {
uint8_t int_buffer[2];
float Int;
@@ -514,6 +317,10 @@
int_buffer[1]=command[2];
Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
KP_LEFT=Int/1000;
+ int32_t data_buff;
+ data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
+ memory.write(ADDRESS_LEFT_KP,data_buff);
+ wait_ms(EEPROM_DELAY);
break;
}
case SET_KI_LEFT: {
@@ -523,6 +330,10 @@
int_buffer[1]=command[2];
Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
KI_LEFT=Int/1000;
+ int32_t data_buff;
+ data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
+ memory.write(ADDRESS_LEFT_KI ,data_buff);
+ wait_ms(EEPROM_DELAY);
break;
}
case SET_KD_LEFT: {
@@ -532,6 +343,10 @@
int_buffer[1]=command[2];
Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
KD_LEFT=Int/1000;
+ int32_t data_buff;
+ data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
+ memory.write(ADDRESS_LEFT_KD,data_buff);
+ wait_ms(EEPROM_DELAY);
break;
}
case SET_KP_RIGHT: {
@@ -541,6 +356,10 @@
int_buffer[1]=command[2];
Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
KP_RIGHT=Int/1000;
+ int32_t data_buff;
+ data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
+ memory.write(ADDRESS_RIGHT_KP,data_buff);
+ wait_ms(EEPROM_DELAY);
break;
}
case SET_KI_RIGHT: {
@@ -550,6 +369,10 @@
int_buffer[1]=command[2];
Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
KI_RIGHT=Int/1000;
+ int32_t data_buff;
+ data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
+ memory.write(ADDRESS_RIGHT_KI,data_buff);
+ wait_ms(EEPROM_DELAY);
break;
}
case SET_KD_RIGHT: {
@@ -559,6 +382,10 @@
int_buffer[1]=command[2];
Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
KD_RIGHT=Int/1000;
+ int32_t data_buff;
+ data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
+ memory.write(ADDRESS_RIGHT_KD,data_buff);
+ wait_ms(EEPROM_DELAY);
break;
}
}
@@ -617,8 +444,9 @@
break;
}
case GET_KP_LEFT: {
+ memory.read(ADDRESS_LEFT_KP ,KP_LEFT_BUFF);
uint8_t intKPL[2],floatKPL[2];
- com.FloatSep(KP_LEFT,intKPL,floatKPL);
+ com.FloatSep(KP_LEFT_BUFF,intKPL,floatKPL);
ANDANTE_PROTOCOL_PACKET package;
@@ -638,8 +466,9 @@
break;
}
case GET_KI_LEFT: {
+ memory.read(ADDRESS_LEFT_KP ,KI_LEFT_BUFF);
uint8_t intKIL[2],floatKIL[2];
- com.FloatSep(KI_LEFT,intKIL,floatKIL);
+ com.FloatSep(KI_LEFT_BUFF,intKIL,floatKIL);
ANDANTE_PROTOCOL_PACKET package;
@@ -659,8 +488,9 @@
break;
}
case GET_KD_LEFT: {
+ memory.read(ADDRESS_LEFT_KP ,KD_LEFT_BUFF);
uint8_t intKDL[2],floatKDL[2];
- com.FloatSep(KD_LEFT,intKDL,floatKDL);
+ com.FloatSep(KD_LEFT_BUFF,intKDL,floatKDL);
ANDANTE_PROTOCOL_PACKET package;
@@ -680,8 +510,9 @@
break;
}
case GET_KP_RIGHT: {
+ memory.read(ADDRESS_LEFT_KP ,KP_RIGHT_BUFF);
uint8_t intKDR[2],floatKDR[2];
- com.FloatSep(KP_RIGHT,intKDR,floatKDR);
+ com.FloatSep(KP_RIGHT_BUFF,intKDR,floatKDR);
ANDANTE_PROTOCOL_PACKET package;
@@ -701,8 +532,9 @@
break;
}
case GET_KI_RIGHT: {
+ memory.read(ADDRESS_LEFT_KP ,KI_RIGHT_BUFF);
uint8_t intKIR[2],floatKIR[2];
- com.FloatSep(KI_RIGHT,intKIR,floatKIR);
+ com.FloatSep(KI_RIGHT_BUFF,intKIR,floatKIR);
ANDANTE_PROTOCOL_PACKET package;
@@ -722,8 +554,9 @@
break;
}
case GET_KD_RIGHT: {
+ memory.read(ADDRESS_LEFT_KP ,KD_RIGHT_BUFF);
uint8_t intKDR[2],floatKDR[2];
- com.FloatSep(KD_RIGHT,intKDR,floatKDR);
+ com.FloatSep(KD_RIGHT_BUFF,intKDR,floatKDR);
ANDANTE_PROTOCOL_PACKET package;