Developers_of_anti_slip_compensator
/
WIPV
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: CURRENT_CONTROL IIR LSM9DS0 MEDIAN_FILTER PID QEI RF24 SENSOR_FUSION mbed
Revision 10:2dc43cd59ff0, committed 2016-08-14
- Comitter:
- adam_z
- Date:
- Sun Aug 14 13:29:50 2016 +0000
- Parent:
- 9:a91135551be1
- Commit message:
- 20160814
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IIR.lib Sun Aug 14 13:29:50 2016 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/LDSC_Robotics_TAs/code/IIR/#3d8226c5abfe
--- a/LSM9DS0.lib Thu Jul 28 08:17:30 2016 +0000 +++ b/LSM9DS0.lib Sun Aug 14 13:29:50 2016 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/teams/LDSC_Robotics_TAs/code/LSM9DS0/#56ff956c499e +https://developer.mbed.org/users/adam_z/code/LSM9DS0/#56ff956c499e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/RF24.lib Sun Aug 14 13:29:50 2016 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/LDSC_Robotics_TAs/code/RF24/#6827b6f0283b
--- a/main.cpp Thu Jul 28 08:17:30 2016 +0000
+++ b/main.cpp Sun Aug 14 13:29:50 2016 +0000
@@ -7,6 +7,10 @@
#include "MEDIAN_FILTER.h"
#include <algorithm>
+#include "nRF24L01.h"
+#include "RF24.h"
+
+
#define Ts 0.001
#define pi 3.14159
@@ -19,6 +23,9 @@
DigitalOut debugLed_l(A4);
DigitalOut debugLed_r(A5);
+DigitalIn BrokenIO(D13);
+
+
InterruptIn SCEnable(USER_BUTTON);
@@ -28,8 +35,19 @@
//void SensorAcquisition(float * data, float samplingTime);
void bluetoothRx();
-void RXIrqLeft();
-void RXIrqRight();
+//void RXIrqLeft();
+//void RXIrqRight();
+
+
+// Set up nRF24L01 radio on SPI bus, and pins 9 (D6) & 10 (A2) on the Shield Shield
+RF24 radio(PB_15, PB_14, PB_13, PB_1, PB_2);//*****MOSI MISO SCK CE CS******
+
+RF24 radio_r(PB_15, PB_14, PB_13, PB_12, PA_11);
+
+const uint64_t left_pipe = 0xF0F0F0F0AA;//These are just arbitrary 64bit numbers to use as pipe identifiers
+const uint64_t right_pipe = 0xF0F0F0F066;//They must be the same on both ends of the communciations
+
+
void SCEnableFunc();
bool SCEnalbeIndicator;
@@ -38,7 +56,7 @@
CURRENT_CONTROL motorCur_L(PC_3, D8, A3,CURRENT_CONTROL::PWM2,400, 900.0,0.0,Ts);
CURRENT_CONTROL motorCur_R(PC_2, D7, D11,CURRENT_CONTROL::PWM1,400, 900.0,0.0,Ts);
-QEI wheel_L(D13, D12, NC, 280, 250, Ts, QEI::X4_ENCODING);
+QEI wheel_L(PC_9, D12, NC, 280, 250, Ts, QEI::X4_ENCODING);
QEI wheel_R(A1, A2, NC, 280, 250, Ts, QEI::X4_ENCODING);
@@ -67,8 +85,8 @@
float ref[4] = {0.0, 0.0, 0.0, 0.0};
float torque_L = 0.0, torque_R = 0.0;
-float KL[4] = {-1.9483 , -0.1317 , 0.0006*2 , -0.0112*2};//{-0.7057 , -0.0733 , -0.0085 , -0.0300};
-float KR[4] = {-1.9483, -0.1317 , -0.0112*2 , 0.0006*2};//{-0.7057 , -0.0733 , -0.0300 , -0.0085};
+float KL[4] = {-0.9666 , -0.1093*0.5 ,0.0,0.0};//-0.0177/2 , -0.0641/2};//{-0.7057 , -0.0733 , -0.0085 , -0.0300};
+float KR[4] = {-0.9666 , -0.1093*0.5 ,0.0,0.0};// -0.0641/2 , -0.0177/2};//{-0.7057 , -0.0733 , -0.0300 , -0.0085};
//********simulated states and multiplying matrices***********************
float z1_prime[4] = {0.0, 0.0, 0.0, 0.0};
@@ -87,6 +105,7 @@
float Km_R = 1.187*0.137;
float yawRate = 0.0;
+float yawFeedback = 0.0;
float velocityCmd[2] = {0.0, 0.0};
unsigned int accelerateFlag = 0, decelerationFlag = 0;
@@ -108,19 +127,50 @@
bool sent_flag = 1;
uint8_t data_sent[18];
-int delay_num = 37;
-float fifo_array[37];
+int delay_num = 3;
+float fifo_array[3];
float a_vehicle_fifo;
int fifo_i = 0;
+int16_t recv_l = 0, recv_r = 0;
+
+float slipVelocity[2] = {0,0};
+
+
+
+int16_t rawData_array[6];
+float data_array[6];
+
+
+
+
int main()
{
pc.baud(230400);
blueTooth.baud(230400);
- LeftSerial.baud(230400); //uart commu with photon left
- RightSerial.baud(230400); //uart commu with photon right
+
+
+ radio.begin();//Start up the radio object
+ radio.setRetries(15,15);//This will improve reliability of the module if it encounters interference
+ radio.setPALevel(RF24_PA_LOW);//This sets the power low. This will reduce the range. RF24_PA_MAX would increase the range
+ radio.setPayloadSize(8);
+ radio.setChannel(76);
+ radio.openReadingPipe(1,left_pipe);
+ radio.startListening();//Give the module a kick
+
+
+ radio_r.begin();//Start up the radio object
+ radio_r.setRetries(15,15);//This will improve reliability of the module if it encounters interference
+ radio_r.setPALevel(RF24_PA_LOW);//This sets the power low. This will reduce the range. RF24_PA_MAX would increase the range
+ radio_r.setPayloadSize(8);
+ radio_r.setChannel(90);//choose a channel different from left channel=76
+ radio_r.openReadingPipe(1,right_pipe);
+ radio_r.startListening();//Give the module a kick
+
+
+
if( sensor.begin() != 0 ) {
pc.printf("Problem starting the sensor with CS @ Pin D6.\r\n");
@@ -135,62 +185,70 @@
WIPVTimer.attach_us(WIPVTimerInterrupt, Ts*1000.0*1000.0);
blueTooth.attach(&bluetoothRx, Serial::RxIrq);
- LeftSerial.attach(&RXIrqLeft, Serial::RxIrq);
- RightSerial.attach(&RXIrqRight, Serial::RxIrq);
+// LeftSerial.attach(&RXIrqLeft, Serial::RxIrq);
+// RightSerial.attach(&RXIrqRight, Serial::RxIrq);
SCEnable.rise(&SCEnableFunc);
SCEnalbeIndicator = 0;
while(true) {
+ //pc.printf("%d, ", rawData_array[0]);
+// pc.printf("%d, ", rawData_array[1]);
+//// pc.printf("%d, ", data_array[2]);
+//// pc.printf("%d, ", data_array[3]);
+// pc.printf("%d, ", rawData_array[4]);
+// pc.printf("%d;\r\n ", rawData_array[5]);
+
+
//pc.printf("%5.4f\t", 10*pitch_angle);
// pc.printf("%5.3f\n", 10*sensor.pitch*3.14159/180);
// pc.printf("%5.3f\r\n", 10*curCmd_L);
-
+
- //if(sent_flag == 0) {///
-// pc.putc(data_sent[send_i]);
-// send_i++;
-// if(send_i >= 18) {
-// send_i = 0;
-// sent_flag = 1;
-// }
-// }///
- pc.printf("%5.4f\t",a_vehicle);
- pc.printf("%5.4f\t",a_vehicle_fifo);
-// pc.printf("%5.4f\t", slipAcceleration_L);
- pc.printf("%5.4f\r\n",slipAcceleration_L);
-// pc.printf("%5.4f\t",yc[0]);
-// pc.printf("%5.4f\r\n",yc[1]);
+ pc.printf("%5.4f\t", state[0]);// pitch angle
+ pc.printf("%5.4f\t", state[1]);//pitch angle rate
+ pc.printf("%5.4f\t", state[2]);//left wheel velocity
+ pc.printf("%5.4f\t", state[3]);//right wheel velocity
+ pc.printf("%5.4f\t", a_slip_L);// left wheel slip acceleration
+ pc.printf("%5.4f\r\n",a_slip_R);// right wheel slip acceleration
+ }
- }
}
-
-void WIPVTimerInterrupt()
-{
- if(tim_counter <100)tim_counter++;
- else if (tim_counter >= 100 && tim_counter <=109) {
- motorCur_L.currentOffset += motorCur_L.currentAnalogIn.read();
- motorCur_R.currentOffset += motorCur_R.currentAnalogIn.read();
- tim_counter++;
- if(tim_counter == 110) {
- motorCur_L.currentOffset = motorCur_L.currentOffset/10;
- motorCur_R.currentOffset = motorCur_R.currentOffset/10;
- }
+ void WIPVTimerInterrupt() {
+ if(tim_counter <100)tim_counter++;
+ else if (tim_counter >= 100 && tim_counter <=109) {
+ motorCur_L.currentOffset += motorCur_L.currentAnalogIn.read();
+ motorCur_R.currentOffset += motorCur_R.currentAnalogIn.read();
+ tim_counter++;
+ if(tim_counter == 110) {
+ motorCur_L.currentOffset = motorCur_L.currentOffset/10;
+ motorCur_R.currentOffset = motorCur_R.currentOffset/10;
+ }
- } else {
-
-
- //int16_t data_array[6];
+ } else {
+
+
+ if( radio.available()) { //Keep checking on each loop to see if any data has come in
+ radio.read(&recv_l, sizeof(int16_t));//Stuff the incoming packet into the motor_code variable
+ }
+// wait_us(100);
+ if( radio_r.available()) { //Keep checking on each loop to see if any data has come in
+ radio_r.read(&recv_r, sizeof(int16_t));//Stuff the incoming packet into the motor_code variable
+ }
+
+
+ //rawData_array[0] = 0;
+// rawData_array[1] = 0;
//
-// data_array[0] = sensor.readRawAccelX();
-// data_array[1] = sensor.readRawAccelY();
-//// data_array[2] = sensor.readRawAccelZ();
-//// data_array[3] = sensor.readRawGyroX();
-// data_array[4] = sensor.readRawGyroY();
-// data_array[5] = sensor.readRawGyroZ();
-//
-// pc.printf("%d, ", data_array[0]);
+// rawData_array[0] = sensor.readRawAccelX();
+// rawData_array[1] = sensor.readRawAccelY();
+//// rawData_array[2] = sensor.readRawAccelZ();
+//// rawData_array[3] = sensor.readRawGyroX();
+// rawData_array[4] = sensor.readRawGyroY();
+// rawData_array[5] = sensor.readRawGyroZ();
+
+ //pc.printf("%d, ", data_array[0]);
// pc.printf("%d, ", data_array[1]);
//// pc.printf("%d, ", data_array[2]);
//// pc.printf("%d, ", data_array[3]);
@@ -199,305 +257,302 @@
- float data_array[6];//Gs and deg/s
- data_array[0] = sensor.readFloatAccelX();
- data_array[1] = sensor.readFloatAccelY();
+ //float data_array[6];//Gs and deg/s
+ data_array[0] = sensor.readFloatAccelX();
+ data_array[1] = sensor.readFloatAccelY();
// data_array[2] = sensor.readFloatAccelZ();
// data_array[3] = sensor.readFloatGyroX();
- data_array[4] = sensor.readFloatGyroY();
- data_array[5] = sensor.readFloatGyroZ();
+ data_array[4] = sensor.readFloatGyroY();
+ data_array[5] = sensor.readFloatGyroZ();
- sensor.complementaryFilter(data_array,Ts);
+ sensor.complementaryFilter(data_array,Ts);
- //===============wheel speed calculation============//
- wheel_L.Calculate();
- wheel_R.Calculate();
+ //===============wheel speed calculation============//
+ wheel_L.Calculate();
+ wheel_R.Calculate();
- /////////////Balancing Control/////////////////////////
- //SI dimension
- state[0] = sensor.pitch*3.14159/180.0;
- state[1] = sensorFilter1.filter(data_array[5]*3.14159/180.0, 6.0);
- state[2] = sensorFilter2.filter(wheel_L.getAngularSpeed(),80.0);
- state[3] = -sensorFilter3.filter(wheel_R.getAngularSpeed(),80.0);
+ /////////////Balancing Control/////////////////////////
+ //SI dimension
+ state[0] = sensor.pitch*3.14159/180.0 +0.08;
+ state[1] = sensorFilter1.filter(data_array[5]*3.14159/180.0, 6.0);
+ state[2] = sensorFilter2.filter(wheel_L.getAngularSpeed(),80.0);
+ state[3] = -sensorFilter3.filter(wheel_R.getAngularSpeed(),80.0);
- if(accelerateFlag == 1) {
- if(velocityCmd[0]>=20 || velocityCmd[1]>=20) {
- accelerateFlag = 0;
- decelerationFlag = 1;
- } else {
- velocityCmd[0] += 0.010;
- velocityCmd[1] += 0.010;
+ if(accelerateFlag == 1) {
+ if(velocityCmd[0]>=20 || velocityCmd[1]>=20) {
+ accelerateFlag = 0;
+ decelerationFlag = 1;
+ } else {
+ velocityCmd[0] += 0.010;
+ velocityCmd[1] += 0.010;
+ }
}
- }
- if(decelerationFlag == 1 & velocityCmd[0] > 0.0) {
- velocityCmd[0] -= 0.015;
- velocityCmd[1] -= 0.015;
- if(velocityCmd[0] <= 0.0)decelerationFlag = 0;
- }
+ if(decelerationFlag == 1 & velocityCmd[0] > 0.0) {
+ velocityCmd[0] -= 0.015;
+ velocityCmd[1] -= 0.015;
+ if(velocityCmd[0] <= 0.0)decelerationFlag = 0;
+ }
- ref[2] = velocityCmd[0];
- ref[3] = velocityCmd[1];
+ ref[2] = velocityCmd[0];
+ ref[3] = velocityCmd[1];
- yawRate = sensorFilter4.filter(data_array[4],9);
+ yawRate = sensorFilter4.filter(data_array[4],9);
- //===============integration of z1_prime=========================
- z1_prime[0] += z1_prime_dot[0]*Ts;
- z1_prime[1] += z1_prime_dot[1]*Ts;
- z1_prime[2] += z1_prime_dot[2]*Ts;
- z1_prime[3] += z1_prime_dot[3]*Ts;
+ //===============integration of z1_prime=========================
+ z1_prime[0] += z1_prime_dot[0]*Ts;
+ z1_prime[1] += z1_prime_dot[1]*Ts;
+ z1_prime[2] += z1_prime_dot[2]*Ts;
+ z1_prime[3] += z1_prime_dot[3]*Ts;
- yc[0] = (KL[0]*(ref[0] - (state[0] - z1_prime[0])) + KL[1]*(ref[1] - (state[1] - z1_prime[1]))\
- + KL[2]*(ref[2] - (state[2] - z1_prime[2])) + KL[3]*(ref[3] - (state[3] - z1_prime[3])));
+ yc[0] = (KL[0]*(ref[0] - (state[0] - z1_prime[0])) + KL[1]*(ref[1] - (state[1] - z1_prime[1]))\
+ + KL[2]*(ref[2] - (state[2] - z1_prime[2])) + KL[3]*(ref[3] - (state[3] - z1_prime[3])));
- yc[1] = (KR[0]*(ref[0] - (state[0] - z1_prime[0])) + KR[1]*(ref[1] - (state[1] - z1_prime[1]))\
- + KR[2]*(ref[2] - (state[2] - z1_prime[2])) + KR[3]*(ref[3] - (state[3] - z1_prime[3])));
+ yc[1] = (KR[0]*(ref[0] - (state[0] - z1_prime[0])) + KR[1]*(ref[1] - (state[1] - z1_prime[1]))\
+ + KR[2]*(ref[2] - (state[2] - z1_prime[2])) + KR[3]*(ref[3] - (state[3] - z1_prime[3])));
- //=========================Anti slip control=============================//
- //***********filtering the slip acceleration of two wheels********************
- a_vehicle = (data_array[0]*cos(state[0]) - data_array[1]*sin(state[0]))*9.791;//compute acceleration of vehicle using lsm9ds0
- //fifo delay
- a_vehicle_fifo = fifo_array[fifo_i];
+ //=========================Anti slip control=============================//
+ //***********filtering the slip acceleration of two wheels********************
+ a_vehicle = (data_array[0]*cos(state[0]) - data_array[1]*sin(state[0]))*9.791;//compute acceleration of vehicle using lsm9ds0
+ //fifo delay
+ a_vehicle_fifo = fifo_array[fifo_i];
// delay_num = sizeof(fifo_array);
- fifo_array[fifo_i] = a_vehicle;
- fifo_i++;
- if(fifo_i >= delay_num)fifo_i = 0;
+ fifo_array[fifo_i] = a_vehicle;
+ fifo_i++;
+ if(fifo_i >= delay_num)fifo_i = 0;
- //pc.printf("%5.2f\t", a_vehicle);
+ //pc.printf("%5.2f\t", a_vehicle);
//// pc.printf("%5.2f\t",slipAcceleration_L_f);
// pc.printf("%5.2f\r\n",slipAcceleration_R_f);
- a_slip_L = slipAcceleration_L - a_vehicle_fifo;
- a_slip_R = slipAcceleration_R - a_vehicle_fifo;
+ a_slip_L = ((float)recv_l)/1000.0 - a_vehicle_fifo;
+ a_slip_R = -((float)recv_r)/1000.0 - a_vehicle_fifo;
- float alpha = 40.0;//HZ
- slipAcceleration_L_f = (1 - alpha*2*3.141593*Ts)*slipAcceleration_L_f + alpha*2*3.141593*Ts*a_slip_L;
- slipAcceleration_R_f = (1 - alpha*2*3.141593*Ts)*slipAcceleration_R_f + alpha*2*3.141593*Ts*a_slip_R;
+ float alpha = 200.0;//HZ
+ slipAcceleration_L_f = (1 - alpha*2*3.141593*Ts)*slipAcceleration_L_f + alpha*2*3.141593*Ts*a_slip_L;
+ slipAcceleration_R_f = (1 - alpha*2*3.141593*Ts)*slipAcceleration_R_f + alpha*2*3.141593*Ts*a_slip_R;
- if(SCEnalbeIndicator == 0) {
- slipAcceleration_L_f_d = 0;
- slipAcceleration_R_f_d = 0;
+ if(SCEnalbeIndicator == 0) {
+ slipAcceleration_L_f_d = 0;
+ slipAcceleration_R_f_d = 0;
- } else {
- //***********dead zone************************//
- float deadzone = 0.0;
- if(slipAcceleration_L_f < deadzone & slipAcceleration_L_f > -deadzone)slipAcceleration_L_f_d = 0;
- else slipAcceleration_L_f_d = slipAcceleration_L_f;
+ } else {
+ //***********dead zone************************//
+ float deadzone = 0.0;
+ if(slipAcceleration_L_f < deadzone & slipAcceleration_L_f > -deadzone)slipAcceleration_L_f_d = 0;
+ else slipAcceleration_L_f_d = slipAcceleration_L_f;
- if(slipAcceleration_R_f < deadzone & slipAcceleration_R_f > -deadzone)slipAcceleration_R_f_d = 0;
- else slipAcceleration_R_f_d = slipAcceleration_R_f;
+ if(slipAcceleration_R_f < deadzone & slipAcceleration_R_f > -deadzone)slipAcceleration_R_f_d = 0;
+ else slipAcceleration_R_f_d = slipAcceleration_R_f;
- }
+ }
- //***************simulated plant feedback**************
- v1[0] = -(Ks0[0]*z1_prime[0] + Ks0[1]*z1_prime[1] + Ks0[2]*z1_prime[2] + Ks0[3]*z1_prime[3]);
- v1[1] = -(Ks1[0]*z1_prime[0] + Ks1[1]*z1_prime[1] + Ks1[2]*z1_prime[2] + Ks1[3]*z1_prime[3]);
+ //***************simulated plant feedback**************
+ v1[0] = -(Ks0[0]*z1_prime[0] + Ks0[1]*z1_prime[1] + Ks0[2]*z1_prime[2] + Ks0[3]*z1_prime[3]);
+ v1[1] = -(Ks1[0]*z1_prime[0] + Ks1[1]*z1_prime[1] + Ks1[2]*z1_prime[2] + Ks1[3]*z1_prime[3]);
- //**********simulated dynamics*****************
- z1_prime_dot[0] = A1_prime0[0]*z1_prime[0] + A1_prime0[1]*z1_prime[1]+A1_prime0[2]*z1_prime[2]+A1_prime0[3]*z1_prime[3]\
- + B11_prime0[0]*v1[0]+ B11_prime0[1]*v1[1]\
- + B31_0[0]*slipAcceleration_L_f_d+ B31_0[1]*slipAcceleration_R_f_d;
+ //**********simulated dynamics*****************
+ z1_prime_dot[0] = A1_prime0[0]*z1_prime[0] + A1_prime0[1]*z1_prime[1]+A1_prime0[2]*z1_prime[2]+A1_prime0[3]*z1_prime[3]\
+ + B11_prime0[0]*v1[0]+ B11_prime0[1]*v1[1]\
+ + B31_0[0]*slipAcceleration_L_f_d+ B31_0[1]*slipAcceleration_R_f_d;
+
+ z1_prime_dot[1] = A1_prime1[0]*z1_prime[0] + A1_prime1[1]*z1_prime[1]+A1_prime1[2]*z1_prime[2]+A1_prime1[3]*z1_prime[3]\
+ + B11_prime1[0]*v1[0]+ B11_prime1[1]*v1[1]\
+ + B31_1[0]*slipAcceleration_L_f_d+ B31_1[1]*slipAcceleration_R_f_d;
- z1_prime_dot[1] = A1_prime1[0]*z1_prime[0] + A1_prime1[1]*z1_prime[1]+A1_prime1[2]*z1_prime[2]+A1_prime1[3]*z1_prime[3]\
- + B11_prime1[0]*v1[0]+ B11_prime1[1]*v1[1]\
- + B31_1[0]*slipAcceleration_L_f_d+ B31_1[1]*slipAcceleration_R_f_d;
+ z1_prime_dot[2] = A1_prime2[0]*z1_prime[0] + A1_prime2[1]*z1_prime[1]+A1_prime2[2]*z1_prime[2]+A1_prime2[3]*z1_prime[3]\
+ + B11_prime2[0]*v1[0]+ B11_prime2[1]*v1[1]\
+ + B31_2[0]*slipAcceleration_L_f_d+ B31_2[1]*slipAcceleration_R_f_d;
- z1_prime_dot[2] = A1_prime2[0]*z1_prime[0] + A1_prime2[1]*z1_prime[1]+A1_prime2[2]*z1_prime[2]+A1_prime2[3]*z1_prime[3]\
- + B11_prime2[0]*v1[0]+ B11_prime2[1]*v1[1]\
- + B31_2[0]*slipAcceleration_L_f_d+ B31_2[1]*slipAcceleration_R_f_d;
+ z1_prime_dot[3] = A1_prime3[0]*z1_prime[0] + A1_prime3[1]*z1_prime[1]+A1_prime3[2]*z1_prime[2]+A1_prime3[3]*z1_prime[3]\
+ + B11_prime3[0]*v1[0]+ B11_prime3[1]*v1[1]\
+ + B31_3[0]*slipAcceleration_L_f_d+ B31_3[1]*slipAcceleration_R_f_d;
+
- z1_prime_dot[3] = A1_prime3[0]*z1_prime[0] + A1_prime3[1]*z1_prime[1]+A1_prime3[2]*z1_prime[2]+A1_prime3[3]*z1_prime[3]\
- + B11_prime3[0]*v1[0]+ B11_prime3[1]*v1[1]\
- + B31_3[0]*slipAcceleration_L_f_d+ B31_3[1]*slipAcceleration_R_f_d;
-
- torque_L = -((yc[0] + v1[0])/Km_L) + 0.015*yawRate;
- torque_R = ((yc[1] + v1[1])/Km_R) + 0.015*yawRate;
+ yawFeedback += 0.004*yawRate*Ts;
+
+ torque_L = -((yc[0] + v1[0])/Km_L) + 0.008*yawRate + yawFeedback;
+ torque_R = ((yc[1] + v1[1])/Km_R) + 0.008*yawRate + yawFeedback;
- //************motor torque control*****************
-// motorCur_L.Control(saturation(torque_L,1.3,-1.3), state[2]);
-// motorCur_R.Control(saturation(torque_R,1.3,-1.3), -state[3]);
+ //************motor torque control*****************
+ motorCur_L.Control(saturation(torque_L,1.3,-1.3), state[2]);
+ motorCur_R.Control(saturation(torque_R,1.3,-1.3), -state[3]);
+
+ //**************sending data to PC******************
+ if(send_counter < 40)send_counter++;
- //**************sending data to PC******************
- if(send_counter < 40)send_counter++;
-
- else if( sent_flag == 1 & send_counter >= 40) {
- send_counter = 0;
- int multiplier = 500;
- data_sent[0] = 0xAA;
- data_sent[1] = 0x55;
+ else if( sent_flag == 1 & send_counter >= 40) {
+ send_counter = 0;
+ int multiplier = 500;
+ data_sent[0] = 0xAA;
+ data_sent[1] = 0x55;
- data_sent[2] = ((int16_t)((state[0])*multiplier)) >>8;
- data_sent[3] = ((int16_t)((state[0])*multiplier)) & 0x00FF;
- data_sent[4] = ((int16_t)(state[1]*multiplier)) >>8;
- data_sent[5] = ((int16_t)(state[1]*multiplier)) & 0x00FF;
+ data_sent[2] = ((int16_t)((state[0])*multiplier)) >>8;
+ data_sent[3] = ((int16_t)((state[0])*multiplier)) & 0x00FF;
+ data_sent[4] = ((int16_t)(state[1]*multiplier)) >>8;
+ data_sent[5] = ((int16_t)(state[1]*multiplier)) & 0x00FF;
- data_sent[6] = ((int16_t)((v1[0])*multiplier)) >>8;
- data_sent[7] = ((int16_t)((v1[0])*multiplier)) & 0x00FF;
- data_sent[8] = ((int16_t)((v1[1])*multiplier)) >>8;
- data_sent[9] = ((int16_t)((v1[1])*multiplier)) & 0x00FF;
+ data_sent[6] = ((int16_t)((v1[0])*multiplier)) >>8;
+ data_sent[7] = ((int16_t)((v1[0])*multiplier)) & 0x00FF;
+ data_sent[8] = ((int16_t)((v1[1])*multiplier)) >>8;
+ data_sent[9] = ((int16_t)((v1[1])*multiplier)) & 0x00FF;
- data_sent[10] = ((int16_t)((yc[0])*multiplier)) >>8;
- data_sent[11] = ((int16_t)((yc[0])*multiplier)) & 0x00FF;
- data_sent[12] = ((int16_t)((yc[1])*multiplier)) >>8;
- data_sent[13] = ((int16_t)((yc[1])*multiplier)) & 0x00FF;
- //data_sent[10] = ((int16_t)((-a_vehicle)*multiplier)) >>8;
+ data_sent[10] = ((int16_t)((yc[0])*multiplier)) >>8;
+ data_sent[11] = ((int16_t)((yc[0])*multiplier)) & 0x00FF;
+ data_sent[12] = ((int16_t)((yc[1])*multiplier)) >>8;
+ data_sent[13] = ((int16_t)((yc[1])*multiplier)) & 0x00FF;
+ //data_sent[10] = ((int16_t)((-a_vehicle)*multiplier)) >>8;
// data_sent[11] = ((int16_t)((-a_vehicle)*multiplier)) & 0x00FF;
// data_sent[12] = ((int16_t)((slipAcceleration_R_m)*multiplier)) >>8;
// data_sent[13] = ((int16_t)((slipAcceleration_R_m)*multiplier)) & 0x00FF;
- data_sent[14] = ((int16_t)((slipAcceleration_L)*multiplier)) >>8;
- data_sent[15] = ((int16_t)((slipAcceleration_L)*multiplier)) & 0x00FF;
- data_sent[16] = ((int16_t)((slipAcceleration_R)*multiplier)) >>8;
- data_sent[17] = ((int16_t)((slipAcceleration_R)*multiplier)) & 0x00FF;
+ data_sent[14] = ((int16_t)((slipAcceleration_L)*multiplier)) >>8;
+ data_sent[15] = ((int16_t)((slipAcceleration_L)*multiplier)) & 0x00FF;
+ data_sent[16] = ((int16_t)((slipAcceleration_R)*multiplier)) >>8;
+ data_sent[17] = ((int16_t)((slipAcceleration_R)*multiplier)) & 0x00FF;
- sent_flag = 0;
+ sent_flag = 0;
- }
+ }
- /* //PD Balancing Control
- balancingPD.Compute(0.0, sensor.pitch*3.14159/180);
- curCmd_R = sensorFilter.filter(saturation(0.5*( -balancingPD.output + 0.002*data_array[5]),1.0, -1.0),10);
- //======================current control=========================//
- tim += Ts;
- if(tim >= 4*pi/omega)tim = 0.0;
- //curCmd_R = amp*sin(omega*tim); //current command
- //curCmd_L = 0.8;
+ /* //PD Balancing Control
+ balancingPD.Compute(0.0, sensor.pitch*3.14159/180);
+ curCmd_R = sensorFilter.filter(saturation(0.5*( -balancingPD.output + 0.002*data_array[5]),1.0, -1.0),10);
+ //======================current control=========================//
+ tim += Ts;
+ if(tim >= 4*pi/omega)tim = 0.0;
+ //curCmd_R = amp*sin(omega*tim); //current command
+ //curCmd_L = 0.8;
- motorCur_R.SetPWMDuty(0.75);
+ motorCur_R.SetPWMDuty(0.75);
- motorCur_L.Control(-curCmd_R + 0.002*data_array[4], wheel_L.getAngularSpeed());
- motorCur_R.Control(curCmd_R + 0.002*data_array[4], wheel_R.getAngularSpeed());
- */
+ motorCur_L.Control(-curCmd_R + 0.002*data_array[4], wheel_L.getAngularSpeed());
+ motorCur_R.Control(curCmd_R + 0.002*data_array[4], wheel_R.getAngularSpeed());
+ */
+
+ }
+
}
-}
-
-
-void bluetoothRx()
-{
- while(pc.readable()) {
- char charRx = pc.getc();
- switch(charRx) {
- case 'w'://forward
- velocityCmd[0] = 2.5;
- velocityCmd[1] = 2.5;
- accelerateFlag = 0;
- break;
- case 's'://backward
- velocityCmd[0] = -3.0;
- velocityCmd[1] = -3.0;
- accelerateFlag = 0;
- break;
- case 'a'://turn left
- velocityCmd[0] = -4.0;
- velocityCmd[1] = 4.0;
- accelerateFlag = 0;
- break;
- case 'd'://turn right
- velocityCmd[0] = 4.0;
- velocityCmd[1] = -4.0;
- accelerateFlag = 0;
- break;
- case 'x'://stop
- velocityCmd[0] = 0.0;
- velocityCmd[1] = 0.0;
- accelerateFlag = 0;
- break;
+ void bluetoothRx() {
+ while(blueTooth.readable()) {
+ char charRx = blueTooth.getc();
+// debugLed_l = !debugLed_l;
+ switch(charRx) {
+ case 'w'://forward
+ ref[0] = 0.30;
+ break;
+ case 's'://backward
+ ref[0] = -0.3;
+ break;
+ //case 'a'://turn left
+// velocityCmd[0] = -4.0;
+// velocityCmd[1] = 4.0;
+// accelerateFlag = 0;
+// break;
+// case 'd'://turn right
+// velocityCmd[0] = 4.0;
+// velocityCmd[1] = -4.0;
+// accelerateFlag = 0;
+// break;
+ case 'x'://stop
+ //velocityCmd[0] = 0.0;
+// velocityCmd[1] = 0.0;
+// accelerateFlag = 0;
+ ref[0] = 0.0;
+ break;
- case 'q'://accelerate
-// debugLed_l = !debugLed_l;
- accelerateFlag = 1;
- break;
- }
- }
-}
-
-void RXIrqLeft()
-{
-
-
- if(LeftSerial.readable()) {
- debugLed_l = !debugLed_l;
- uint8_t c = LeftSerial.getc();
- cLast_l = c;
-
- if( c == 0xAA) {
- i_l = 0;
- headerCaptured_l = 1;
-
-
- } else if(headerCaptured_l) {
- fromPhoton_l[i_l] = c;
- i_l++;
- if(i_l == 2) {
- headerCaptured_l = 0;
- slipAcceleration_L = (float)((int16_t)(fromPhoton_l[0]*256+fromPhoton_l[1])*0.92)/1000.0;
-
-// pc.printf("%5.4f\t",(float)slipAcceleration_L_f);
-// pc.printf("%5.4f\r\n",(float)slipAcceleration_R_f);
+ //case 'q'://accelerate
+//// debugLed_l = !debugLed_l;
+// accelerateFlag = 1;
+// break;
}
-
}
}
-
-}
-
+ //void RXIrqLeft() {
+//
+//
+// if(LeftSerial.readable()) {
+// debugLed_l = !debugLed_l;
+// uint8_t c = LeftSerial.getc();
+// cLast_l = c;
+//
+// if( c == 0xAA) {
+// i_l = 0;
+// headerCaptured_l = 1;
+//
+//
+// } else if(headerCaptured_l) {
+// fromPhoton_l[i_l] = c;
+// i_l++;
+// if(i_l == 2) {
+// headerCaptured_l = 0;
+// slipAcceleration_L = (float)((int16_t)(fromPhoton_l[0]*256+fromPhoton_l[1])*0.92)/1000.0;
+//
+//// pc.printf("%5.4f\t",(float)slipAcceleration_L_f);
+//// pc.printf("%5.4f\r\n",(float)slipAcceleration_R_f);
+// }
+//
+// }
+// }
+//
+//
+// }
+//
+//
+//
+//
+// void RXIrqRight() {
+//
+// // slipAcceleration_L += 0.1;
+// if(RightSerial.readable()) {
+// debugLed_r = !debugLed_r;
+// uint8_t c = RightSerial.getc();
+// cLast_r = c;
+//// pc.printf("%5.4f\r\n",(float)cLast_r);
+//
+// if( c == 0xAA) {
+// headerCaptured_r = 1;
+// i_r = 0;
+//
+// } else if(headerCaptured_r) {
+//
+// fromPhoton_r[i_r] = c;
+// i_r++;
+// if(i_r == 2) {
+// headerCaptured_r = 0;
+// slipAcceleration_R = -(float)((int16_t)(fromPhoton_r[0]*256+fromPhoton_r[1])*1.18)/1000.0;
+//
+// }
+// }
+// }
+// }
-void RXIrqRight()
-{
-
- // slipAcceleration_L += 0.1;
- if(RightSerial.readable()) {
- debugLed_r = !debugLed_r;
- uint8_t c = RightSerial.getc();
- cLast_r = c;
-// pc.printf("%5.4f\r\n",(float)cLast_r);
-
- if( c == 0xAA) {
- headerCaptured_r = 1;
- i_r = 0;
-
- } else if(headerCaptured_r) {
-
- fromPhoton_r[i_r] = c;
- i_r++;
- if(i_r == 2) {
- headerCaptured_r = 0;
- slipAcceleration_R = -(float)((int16_t)(fromPhoton_r[0]*256+fromPhoton_r[1])*1.18)/1000.0;
-
- }
- }
+ float saturation(float input, float limit_H, float limit_L) {
+ float output;
+ if(input >= limit_H)output = limit_H;
+ else if (input <= limit_L)output = limit_L;
+ else output = input;
+ return output;
}
-}
-
-
-
-float saturation(float input, float limit_H, float limit_L)
-{
- float output;
- if(input >= limit_H)output = limit_H;
- else if (input <= limit_L)output = limit_L;
- else output = input;
- return output;
-}
//void SensorAcquisition(float * data, float samplingTime)
@@ -526,9 +581,9 @@
//}
-void SCEnableFunc()
-{
+ void SCEnableFunc() {
// debugLed_l = !debugLed_l;
- accelerateFlag = 1;
- SCEnalbeIndicator = !SCEnalbeIndicator;
-}
+// accelerateFlag = 1;
+// ref[0] = 0.2;
+ SCEnalbeIndicator = !SCEnalbeIndicator;
+ }
