Balancing_Robot_2 - First trial

Users » lakshmananag » Code » Balancing_Robot_2

Lakshmanan Gopalakrishnan / Mbed 2 deprecated Balancing_Robot_2

First trial

Dependencies: MPU6050 Motor ledControl2 mbed

Fork of BalancingRobotPS3 by Kristian Lauszus

BalancingRobot.cpp@7:f1d24c6119ac, 2012-04-09 (annotated)

Committer:: Lauszus
Date:: Mon Apr 09 21:40:47 2012 +0000
Revision:: 7:f1d24c6119ac
Parent:: 6:defe36ce2346
Child:: 9:67f2110fce8e

Who changed what in which revision?

User	Revision	Line number	New contents of line
Lauszus	4:0b4c320bc948	1	/*
Lauszus	4:0b4c320bc948	2	* The code is released under the GNU General Public License.
Lauszus	4:0b4c320bc948	3	* Developed by Kristian Lauszus
Lauszus	4:0b4c320bc948	4	* This is the algorithm for my balancing robot/segway.
Lauszus	4:0b4c320bc948	5	* It is controlled by a PS3 Controller via bluetooth.
Lauszus	4:0b4c320bc948	6	* The remote control code can be found at my other github repository: https://github.com/TKJElectronics/BalancingRobot
Lauszus	4:0b4c320bc948	7	* For details, see http://blog.tkjelectronics.dk/2012/02/the-balancing-robot/
Lauszus	4:0b4c320bc948	8	*/
Lauszus	4:0b4c320bc948	9
Lauszus	4:0b4c320bc948	10	#include "mbed.h"
Lauszus	4:0b4c320bc948	11	#include "BalancingRobot.h"
Lauszus	4:0b4c320bc948	12	#include "Encoder.h"
Lauszus	4:0b4c320bc948	13
Lauszus	4:0b4c320bc948	14	/* Setup serial communication */
Lauszus	4:0b4c320bc948	15	Serial xbee(p13,p14); // For wireless debugging and setting PID constants
Lauszus	4:0b4c320bc948	16	Serial ps3(p9,p10); // For remote control
Lauszus	4:0b4c320bc948	17	Serial debug(USBTX, USBRX); // USB
Lauszus	4:0b4c320bc948	18
Lauszus	4:0b4c320bc948	19	/* Setup encoders */
Lauszus	4:0b4c320bc948	20	Encoder leftEncoder(p29,p30);
Lauszus	4:0b4c320bc948	21	Encoder rightEncoder(p28,p27);
Lauszus	4:0b4c320bc948	22
Lauszus	4:0b4c320bc948	23	/* Setup timer instance */
Lauszus	4:0b4c320bc948	24	Timer t;
Lauszus	4:0b4c320bc948	25
Lauszus	4:0b4c320bc948	26	int main() {
Lauszus	4:0b4c320bc948	27	/* Set baudrate */
Lauszus	4:0b4c320bc948	28	xbee.baud(115200);
Lauszus	4:0b4c320bc948	29	ps3.baud(115200);
Lauszus	4:0b4c320bc948	30	debug.baud(115200);
Lauszus	7:f1d24c6119ac	31
Lauszus	4:0b4c320bc948	32	/* Set PWM frequency */
Lauszus	4:0b4c320bc948	33	leftPWM.period(0.00005); // The motor driver can handle a pwm frequency up to 20kHz (1/20000=0.00005)
Lauszus	4:0b4c320bc948	34	rightPWM.period(0.00005);
Lauszus	7:f1d24c6119ac	35
Lauszus	4:0b4c320bc948	36	/* Calibrate the gyro and accelerometer relative to ground */
Lauszus	4:0b4c320bc948	37	calibrateSensors();
Lauszus	7:f1d24c6119ac	38
Lauszus	4:0b4c320bc948	39	xbee.printf("Initialized\n");
Lauszus	4:0b4c320bc948	40	processing(); // Send output to processing application
Lauszus	7:f1d24c6119ac	41
Lauszus	4:0b4c320bc948	42	/* Setup timing */
Lauszus	4:0b4c320bc948	43	t.start();
Lauszus	4:0b4c320bc948	44	loopStartTime = t.read_us();
Lauszus	4:0b4c320bc948	45	timer = loopStartTime;
Lauszus	7:f1d24c6119ac	46
Lauszus	4:0b4c320bc948	47	while (1) {
Lauszus	4:0b4c320bc948	48	/* Calculate pitch */
Lauszus	4:0b4c320bc948	49	accYangle = getAccY();
Lauszus	4:0b4c320bc948	50	gyroYrate = getGyroYrate();
Lauszus	7:f1d24c6119ac	51
Lauszus	4:0b4c320bc948	52	// See my guide for more info about calculation the angles and the Kalman filter: http://arduino.cc/forum/index.php/topic,58048.0.html
Lauszus	4:0b4c320bc948	53	pitch = kalman(accYangle, gyroYrate, t.read_us() - timer); // calculate the angle using a Kalman filter
Lauszus	4:0b4c320bc948	54	timer = t.read_us();
Lauszus	7:f1d24c6119ac	55
Lauszus	4:0b4c320bc948	56	//debug.printf("Pitch: %f, accYangle: %f\n",pitch,accYangle);
Lauszus	7:f1d24c6119ac	57
Lauszus	4:0b4c320bc948	58	/* Drive motors */
Lauszus	4:0b4c320bc948	59	if (pitch < 60 \|\| pitch > 120) // Stop if falling or laying down
Lauszus	4:0b4c320bc948	60	stopAndReset();
Lauszus	4:0b4c320bc948	61	else
Lauszus	4:0b4c320bc948	62	PID(targetAngle,targetOffset);
Lauszus	7:f1d24c6119ac	63
Lauszus	4:0b4c320bc948	64	/* Update wheel velocity every 100ms */
Lauszus	4:0b4c320bc948	65	loopCounter++;
Lauszus	4:0b4c320bc948	66	if (loopCounter%10 == 0) { // If remainder is equal 0, it must be 10,20,30 etc.
Lauszus	4:0b4c320bc948	67	//xbee.printf("Wheel - timer: %i\n",t.read_ms());
Lauszus	4:0b4c320bc948	68	wheelPosition = leftEncoder.read() + rightEncoder.read();
Lauszus	4:0b4c320bc948	69	wheelVelocity = wheelPosition - lastWheelPosition;
Lauszus	4:0b4c320bc948	70	lastWheelPosition = wheelPosition;
Lauszus	4:0b4c320bc948	71	//xbee.printf("WheelVelocity: %i\n",wheelVelocity);
Lauszus	7:f1d24c6119ac	72
Lauszus	4:0b4c320bc948	73	if (abs(wheelVelocity) <= 20 && !stopped) { // Set new targetPosition if breaking
Lauszus	4:0b4c320bc948	74	targetPosition = wheelPosition;
Lauszus	4:0b4c320bc948	75	stopped = true;
Lauszus	4:0b4c320bc948	76	//xbee.printf("Stopped\n");
Lauszus	4:0b4c320bc948	77	}
Lauszus	4:0b4c320bc948	78	}
Lauszus	7:f1d24c6119ac	79
Lauszus	4:0b4c320bc948	80	/* Check for incoming serial data */
Lauszus	4:0b4c320bc948	81	if (ps3.readable()) // Check if there's any incoming data
Lauszus	4:0b4c320bc948	82	receivePS3();
Lauszus	4:0b4c320bc948	83	if (xbee.readable()) // For setting the PID values
Lauszus	4:0b4c320bc948	84	receiveXbee();
Lauszus	7:f1d24c6119ac	85
Lauszus	4:0b4c320bc948	86	/* Read battery voltage every 1s */
Lauszus	4:0b4c320bc948	87	if (loopCounter == 100) {
Lauszus	4:0b4c320bc948	88	loopCounter = 0; // Reset loop counter
Lauszus	4:0b4c320bc948	89	double analogVoltage = batteryVoltage.read()/1*3.3; // Convert to voltage
Lauszus	5:fc6c7a059759	90	analogVoltage *= 6.6; // The analog pin is connected to a 56k-10k voltage divider
Lauszus	4:0b4c320bc948	91	xbee.printf("analogVoltage: %f - timer: %i\n",analogVoltage,t.read_ms());
Lauszus	6:defe36ce2346	92	if (analogVoltage < 9 && pitch > 60 && pitch < 120) // Set buzzer on, if voltage gets critical low
Lauszus	7:f1d24c6119ac	93	buzzer = 1; // The mbed resets at aproximatly 8.5V
Lauszus	4:0b4c320bc948	94	}
Lauszus	7:f1d24c6119ac	95
Lauszus	4:0b4c320bc948	96	/* Use a time fixed loop */
Lauszus	4:0b4c320bc948	97	lastLoopUsefulTime = t.read_us() - loopStartTime;
Lauszus	4:0b4c320bc948	98	if (lastLoopUsefulTime < STD_LOOP_TIME)
Lauszus	4:0b4c320bc948	99	wait_us(STD_LOOP_TIME - lastLoopUsefulTime);
Lauszus	4:0b4c320bc948	100	lastLoopTime = t.read_us() - loopStartTime; // only used for debugging
Lauszus	4:0b4c320bc948	101	loopStartTime = t.read_us();
Lauszus	7:f1d24c6119ac	102
Lauszus	4:0b4c320bc948	103	//debug.printf("%i,%i\n",lastLoopUsefulTime,lastLoopTime);
Lauszus	4:0b4c320bc948	104	}
Lauszus	4:0b4c320bc948	105	}
Lauszus	4:0b4c320bc948	106	void PID(double restAngle, double offset) {
Lauszus	4:0b4c320bc948	107	/* Steer robot */
Lauszus	4:0b4c320bc948	108	if (steerForward) {
Lauszus	4:0b4c320bc948	109	offset += (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing
Lauszus	4:0b4c320bc948	110	restAngle -= offset;
Lauszus	4:0b4c320bc948	111	//xbee.printf("Forward offset: %f\t WheelVelocity: %i\n",offset,wheelVelocity);
Lauszus	4:0b4c320bc948	112	} else if (steerBackward) {
Lauszus	4:0b4c320bc948	113	offset -= (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing
Lauszus	4:0b4c320bc948	114	restAngle += offset;
Lauszus	4:0b4c320bc948	115	//xbee.printf("Backward offset: %f\t WheelVelocity: %i\n",offset,wheelVelocity);
Lauszus	4:0b4c320bc948	116	}
Lauszus	4:0b4c320bc948	117	/* Break */
Lauszus	4:0b4c320bc948	118	else if (steerStop) {
Lauszus	4:0b4c320bc948	119	long positionError = wheelPosition - targetPosition;
Lauszus	4:0b4c320bc948	120	if (abs(positionError) > zoneA) // Inside zone A
Lauszus	4:0b4c320bc948	121	restAngle -= (double)positionError/positionScaleA;
Lauszus	4:0b4c320bc948	122	else if (abs(positionError) > zoneB) // Inside zone B
Lauszus	4:0b4c320bc948	123	restAngle -= (double)positionError/positionScaleB;
Lauszus	4:0b4c320bc948	124	else // Inside zone C
Lauszus	4:0b4c320bc948	125	restAngle -= (double)positionError/positionScaleC;
Lauszus	7:f1d24c6119ac	126
Lauszus	4:0b4c320bc948	127	restAngle -= (double)wheelVelocity/velocityScaleStop;
Lauszus	7:f1d24c6119ac	128
Lauszus	4:0b4c320bc948	129	if (restAngle < 80) // Limit rest Angle
Lauszus	4:0b4c320bc948	130	restAngle = 80;
Lauszus	4:0b4c320bc948	131	else if (restAngle > 100)
Lauszus	4:0b4c320bc948	132	restAngle = 100;
Lauszus	4:0b4c320bc948	133	//xbee.printf("restAngle: %f\t WheelVelocity: %i positionError: %i\n",restAngle,wheelVelocity,positionError);
Lauszus	4:0b4c320bc948	134	}
Lauszus	4:0b4c320bc948	135	/* Update PID values */
Lauszus	4:0b4c320bc948	136	double error = (restAngle - pitch)/100;
Lauszus	4:0b4c320bc948	137	double pTerm = Kp * error;
Lauszus	4:0b4c320bc948	138	iTerm += Ki * error;
Lauszus	4:0b4c320bc948	139	double dTerm = Kd * (error - lastError);
Lauszus	4:0b4c320bc948	140	lastError = error;
Lauszus	7:f1d24c6119ac	141
Lauszus	4:0b4c320bc948	142	double PIDValue = pTerm + iTerm + dTerm;
Lauszus	7:f1d24c6119ac	143
Lauszus	4:0b4c320bc948	144	//debug.printf("Pitch: %5.2f\tPIDValue: %5.2f\tpTerm: %5.2f\tiTerm: %5.2f\tdTerm: %5.2f\tKp: %5.2f\tKi: %5.2f\tKd: %5.2f\n",pitch,PIDValue,pTerm,iTerm,dTerm,Kp,Ki,Kd);
Lauszus	7:f1d24c6119ac	145
Lauszus	4:0b4c320bc948	146	/* Steer robot sideways */
Lauszus	4:0b4c320bc948	147	double PIDLeft;
Lauszus	4:0b4c320bc948	148	double PIDRight;
Lauszus	4:0b4c320bc948	149	if (steerLeft) {
Lauszus	4:0b4c320bc948	150	PIDLeft = PIDValue-turnSpeed;
Lauszus	4:0b4c320bc948	151	PIDRight = PIDValue+turnSpeed;
Lauszus	4:0b4c320bc948	152	} else if (steerRotateLeft) {
Lauszus	4:0b4c320bc948	153	PIDLeft = PIDValue-rotateSpeed;
Lauszus	4:0b4c320bc948	154	PIDRight = PIDValue+rotateSpeed;
Lauszus	4:0b4c320bc948	155	} else if (steerRight) {
Lauszus	4:0b4c320bc948	156	PIDLeft = PIDValue+turnSpeed;
Lauszus	4:0b4c320bc948	157	PIDRight = PIDValue-turnSpeed;
Lauszus	4:0b4c320bc948	158	} else if (steerRotateRight) {
Lauszus	4:0b4c320bc948	159	PIDLeft = PIDValue+rotateSpeed;
Lauszus	4:0b4c320bc948	160	PIDRight = PIDValue-rotateSpeed;
Lauszus	4:0b4c320bc948	161	} else {
Lauszus	4:0b4c320bc948	162	PIDLeft = PIDValue;
Lauszus	4:0b4c320bc948	163	PIDRight = PIDValue;
Lauszus	4:0b4c320bc948	164	}
Lauszus	4:0b4c320bc948	165	PIDLeft *= 0.95; // compensate for difference in the motors
Lauszus	7:f1d24c6119ac	166
Lauszus	4:0b4c320bc948	167	/* Set PWM Values */
Lauszus	4:0b4c320bc948	168	if (PIDLeft >= 0)
Lauszus	4:0b4c320bc948	169	move(left, forward, PIDLeft);
Lauszus	4:0b4c320bc948	170	else
Lauszus	4:0b4c320bc948	171	move(left, backward, PIDLeft * -1);
Lauszus	4:0b4c320bc948	172	if (PIDRight >= 0)
Lauszus	4:0b4c320bc948	173	move(right, forward, PIDRight);
Lauszus	4:0b4c320bc948	174	else
Lauszus	4:0b4c320bc948	175	move(right, backward, PIDRight * -1);
Lauszus	4:0b4c320bc948	176	}
Lauszus	4:0b4c320bc948	177	void receivePS3() {
Lauszus	4:0b4c320bc948	178	char input[16]; // The serial buffer is only 16 characters
Lauszus	4:0b4c320bc948	179	int i = 0;
Lauszus	4:0b4c320bc948	180	while (1) {
Lauszus	4:0b4c320bc948	181	input[i] = ps3.getc();
Lauszus	4:0b4c320bc948	182	if (input[i] == ';' \|\| i == 15) // keep reading until it reads a semicolon or it exceeds the serial buffer
Lauszus	4:0b4c320bc948	183	break;
Lauszus	4:0b4c320bc948	184	i++;
Lauszus	4:0b4c320bc948	185	}
Lauszus	4:0b4c320bc948	186	//debug.printf("Input: %s\n",input);
Lauszus	7:f1d24c6119ac	187
Lauszus	4:0b4c320bc948	188	// Set all false
Lauszus	4:0b4c320bc948	189	steerForward = false;
Lauszus	4:0b4c320bc948	190	steerBackward = false;
Lauszus	4:0b4c320bc948	191	steerStop = false;
Lauszus	4:0b4c320bc948	192	steerLeft = false;
Lauszus	4:0b4c320bc948	193	steerRotateLeft = false;
Lauszus	4:0b4c320bc948	194	steerRight = false;
Lauszus	4:0b4c320bc948	195	steerRotateRight = false;
Lauszus	7:f1d24c6119ac	196
Lauszus	4:0b4c320bc948	197	/* For remote control */
Lauszus	4:0b4c320bc948	198	if (input[0] == 'F') { // Forward
Lauszus	4:0b4c320bc948	199	strtok(input, ","); // Ignore 'F'
Lauszus	4:0b4c320bc948	200	targetOffset = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
Lauszus	4:0b4c320bc948	201	//xbee.printf("%f\n",targetOffset); // Print targetOffset for debugging
Lauszus	4:0b4c320bc948	202	steerForward = true;
Lauszus	4:0b4c320bc948	203	} else if (input[0] == 'B') { // Backward
Lauszus	4:0b4c320bc948	204	strtok(input, ","); // Ignore 'B'
Lauszus	4:0b4c320bc948	205	targetOffset = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
Lauszus	4:0b4c320bc948	206	//xbee.printf("%f\n",targetOffset); // Print targetOffset for debugging
Lauszus	4:0b4c320bc948	207	steerBackward = true;
Lauszus	4:0b4c320bc948	208	} else if (input[0] == 'L') { // Left
Lauszus	4:0b4c320bc948	209	if (input[1] == 'R') // Left Rotate
Lauszus	4:0b4c320bc948	210	steerRotateLeft = true;
Lauszus	4:0b4c320bc948	211	else
Lauszus	4:0b4c320bc948	212	steerLeft = true;
Lauszus	4:0b4c320bc948	213	} else if (input[0] == 'R') { // Right
Lauszus	4:0b4c320bc948	214	if (input[1] == 'R') // Right Rotate
Lauszus	4:0b4c320bc948	215	steerRotateRight = true;
Lauszus	4:0b4c320bc948	216	else
Lauszus	4:0b4c320bc948	217	steerRight = true;
Lauszus	4:0b4c320bc948	218	} else if (input[0] == 'S') { // Stop
Lauszus	4:0b4c320bc948	219	steerStop = true;
Lauszus	4:0b4c320bc948	220	stopped = false;
Lauszus	4:0b4c320bc948	221	targetPosition = wheelPosition;
Lauszus	4:0b4c320bc948	222	}
Lauszus	7:f1d24c6119ac	223
Lauszus	4:0b4c320bc948	224	else if (input[0] == 'T') { // Set the target angle
Lauszus	4:0b4c320bc948	225	strtok(input, ","); // Ignore 'T'
Lauszus	4:0b4c320bc948	226	targetAngle = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
Lauszus	4:0b4c320bc948	227	xbee.printf("%f\n",targetAngle); // Print targetAngle for debugging
Lauszus	4:0b4c320bc948	228	} else if (input[0] == 'A') { // Abort
Lauszus	4:0b4c320bc948	229	stopAndReset();
Lauszus	4:0b4c320bc948	230	while (ps3.getc() != 'C'); // Wait until continue is send
Lauszus	4:0b4c320bc948	231	}
Lauszus	4:0b4c320bc948	232	}
Lauszus	4:0b4c320bc948	233	void receiveXbee() {
Lauszus	4:0b4c320bc948	234	char input[16]; // The serial buffer is only 16 characters
Lauszus	4:0b4c320bc948	235	int i = 0;
Lauszus	4:0b4c320bc948	236	while (1) {
Lauszus	4:0b4c320bc948	237	input[i] = xbee.getc();
Lauszus	4:0b4c320bc948	238	if (input[i] == ';' \|\| i == 15) // keep reading until it reads a semicolon or it exceeds the serial buffer
Lauszus	4:0b4c320bc948	239	break;
Lauszus	4:0b4c320bc948	240	i++;
Lauszus	4:0b4c320bc948	241	}
Lauszus	4:0b4c320bc948	242	//debug.printf("Input: %s\n",input);
Lauszus	7:f1d24c6119ac	243
Lauszus	4:0b4c320bc948	244	if (input[0] == 'T') { // Set the target angle
Lauszus	4:0b4c320bc948	245	strtok(input, ","); // Ignore 'T'
Lauszus	4:0b4c320bc948	246	targetAngle = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
Lauszus	4:0b4c320bc948	247	} else if (input[0] == 'P') {
Lauszus	4:0b4c320bc948	248	strtok(input, ",");//Ignore 'P'
Lauszus	4:0b4c320bc948	249	Kp = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
Lauszus	4:0b4c320bc948	250	} else if (input[0] == 'I') {
Lauszus	4:0b4c320bc948	251	strtok(input, ",");//Ignore 'I'
Lauszus	4:0b4c320bc948	252	Ki = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
Lauszus	4:0b4c320bc948	253	} else if (input[0] == 'D') {
Lauszus	4:0b4c320bc948	254	strtok(input, ",");//Ignore 'D'
Lauszus	4:0b4c320bc948	255	Kd = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
Lauszus	4:0b4c320bc948	256	} else if (input[0] == 'A') { // Abort
Lauszus	4:0b4c320bc948	257	stopAndReset();
Lauszus	4:0b4c320bc948	258	while (xbee.getc() != 'C'); // Wait until continue is send
Lauszus	4:0b4c320bc948	259	} else if (input[0] == 'G') // The processing application sends this at startup
Lauszus	4:0b4c320bc948	260	processing(); // Send output to processing application
Lauszus	4:0b4c320bc948	261	}
Lauszus	4:0b4c320bc948	262	void processing() {
Lauszus	4:0b4c320bc948	263	/* Send output to processing application */
Lauszus	4:0b4c320bc948	264	xbee.printf("Processing,%5.2f,%5.2f,%5.2f,%5.2f\n",Kp,Ki,Kd,targetAngle);
Lauszus	4:0b4c320bc948	265	}
Lauszus	4:0b4c320bc948	266	void stopAndReset() {
Lauszus	4:0b4c320bc948	267	stop(both);
Lauszus	4:0b4c320bc948	268	lastError = 0;
Lauszus	4:0b4c320bc948	269	iTerm = 0;
Lauszus	4:0b4c320bc948	270	targetPosition = wheelPosition;
Lauszus	7:f1d24c6119ac	271	buzzer = 0;
Lauszus	4:0b4c320bc948	272	}
Lauszus	4:0b4c320bc948	273	double kalman(double newAngle, double newRate, double dtime) {
Lauszus	4:0b4c320bc948	274	// KasBot V2 - Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145
Lauszus	4:0b4c320bc948	275	// with slightly modifications by Kristian Lauszus
Lauszus	4:0b4c320bc948	276	// See http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf and http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf for more information
Lauszus	4:0b4c320bc948	277	dt = dtime / 1000000; // Convert from microseconds to seconds
Lauszus	7:f1d24c6119ac	278
Lauszus	4:0b4c320bc948	279	// Discrete Kalman filter time update equations - Time Update ("Predict")
Lauszus	4:0b4c320bc948	280	// Update xhat - Project the state ahead
Lauszus	4:0b4c320bc948	281	angle += dt * (newRate - bias);
Lauszus	7:f1d24c6119ac	282
Lauszus	4:0b4c320bc948	283	// Update estimation error covariance - Project the error covariance ahead
Lauszus	4:0b4c320bc948	284	P_00 += -dt * (P_10 + P_01) + Q_angle * dt;
Lauszus	4:0b4c320bc948	285	P_01 += -dt * P_11;
Lauszus	4:0b4c320bc948	286	P_10 += -dt * P_11;
Lauszus	4:0b4c320bc948	287	P_11 += +Q_gyro * dt;
Lauszus	7:f1d24c6119ac	288
Lauszus	4:0b4c320bc948	289	// Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
Lauszus	4:0b4c320bc948	290	// Calculate Kalman gain - Compute the Kalman gain
Lauszus	4:0b4c320bc948	291	S = P_00 + R_angle;
Lauszus	4:0b4c320bc948	292	K_0 = P_00 / S;
Lauszus	4:0b4c320bc948	293	K_1 = P_10 / S;
Lauszus	7:f1d24c6119ac	294
Lauszus	4:0b4c320bc948	295	// Calculate angle and resting rate - Update estimate with measurement zk
Lauszus	4:0b4c320bc948	296	y = newAngle - angle;
Lauszus	4:0b4c320bc948	297	angle += K_0 * y;
Lauszus	4:0b4c320bc948	298	bias += K_1 * y;
Lauszus	7:f1d24c6119ac	299
Lauszus	4:0b4c320bc948	300	// Calculate estimation error covariance - Update the error covariance
Lauszus	4:0b4c320bc948	301	P_00 -= K_0 * P_00;
Lauszus	4:0b4c320bc948	302	P_01 -= K_0 * P_01;
Lauszus	4:0b4c320bc948	303	P_10 -= K_1 * P_00;
Lauszus	4:0b4c320bc948	304	P_11 -= K_1 * P_01;
Lauszus	7:f1d24c6119ac	305
Lauszus	4:0b4c320bc948	306	return angle;
Lauszus	4:0b4c320bc948	307	}
Lauszus	4:0b4c320bc948	308	double getGyroYrate() {
Lauszus	4:0b4c320bc948	309	// (gyroAdc-gyroZero)/Sensitivity (In quids) - Sensitivity = 0.00333/3.3=0.001009091
Lauszus	4:0b4c320bc948	310	double gyroRate = -((gyroY.read() - zeroValues[0]) / 0.001009091);
Lauszus	4:0b4c320bc948	311	return gyroRate;
Lauszus	4:0b4c320bc948	312	}
Lauszus	4:0b4c320bc948	313	double getAccY() {
Lauszus	4:0b4c320bc948	314	// (accAdc-accZero)/Sensitivity (In quids) - Sensitivity = 0.33/3.3=0.1
Lauszus	4:0b4c320bc948	315	double accXval = (accX.read() - zeroValues[1]) / 0.1;
Lauszus	4:0b4c320bc948	316	double accYval = (accY.read() - zeroValues[2]) / 0.1;
Lauszus	4:0b4c320bc948	317	accYval--;//-1g when lying down
Lauszus	4:0b4c320bc948	318	double accZval = (accZ.read() - zeroValues[3]) / 0.1;
Lauszus	7:f1d24c6119ac	319
Lauszus	7:f1d24c6119ac	320	double R = sqrt(pow(accXval, 2) + pow(accYval, 2) + pow(accZval, 2)); // Calculate the length of force vector
Lauszus	4:0b4c320bc948	321	double angleY = acos(accYval / R) * RAD_TO_DEG;
Lauszus	7:f1d24c6119ac	322
Lauszus	4:0b4c320bc948	323	return angleY;
Lauszus	4:0b4c320bc948	324	}
Lauszus	4:0b4c320bc948	325	void calibrateSensors() {
Lauszus	4:0b4c320bc948	326	LEDs = 0xF; // Turn all onboard LEDs on
Lauszus	7:f1d24c6119ac	327
Lauszus	4:0b4c320bc948	328	double adc[4] = {0,0,0,0};
Lauszus	4:0b4c320bc948	329	for (uint8_t i = 0; i < 100; i++) { // Take the average of 100 readings
Lauszus	4:0b4c320bc948	330	adc[0] += gyroY.read();
Lauszus	4:0b4c320bc948	331	adc[1] += accX.read();
Lauszus	4:0b4c320bc948	332	adc[2] += accY.read();
Lauszus	4:0b4c320bc948	333	adc[3] += accZ.read();
Lauszus	4:0b4c320bc948	334	wait_ms(10);
Lauszus	4:0b4c320bc948	335	}
Lauszus	4:0b4c320bc948	336	zeroValues[0] = adc[0] / 100; // Gyro X-axis
Lauszus	4:0b4c320bc948	337	zeroValues[1] = adc[1] / 100; // Accelerometer X-axis
Lauszus	4:0b4c320bc948	338	zeroValues[2] = adc[2] / 100; // Accelerometer Y-axis
Lauszus	4:0b4c320bc948	339	zeroValues[3] = adc[3] / 100; // Accelerometer Z-axis
Lauszus	7:f1d24c6119ac	340
Lauszus	4:0b4c320bc948	341	LEDs = 0x0; // Turn all onboard LEDs off
Lauszus	4:0b4c320bc948	342	}
Lauszus	4:0b4c320bc948	343	void move(Motor motor, Direction direction, float speed) { // speed is a value in percentage (0.0f to 1.0f)
Lauszus	4:0b4c320bc948	344	if (motor == left) {
Lauszus	4:0b4c320bc948	345	leftPWM = speed;
Lauszus	4:0b4c320bc948	346	if (direction == forward) {
Lauszus	4:0b4c320bc948	347	leftA = 0;
Lauszus	4:0b4c320bc948	348	leftB = 1;
Lauszus	4:0b4c320bc948	349	} else if (direction == backward) {
Lauszus	4:0b4c320bc948	350	leftA = 1;
Lauszus	4:0b4c320bc948	351	leftB = 0;
Lauszus	4:0b4c320bc948	352	}
Lauszus	4:0b4c320bc948	353	} else if (motor == right) {
Lauszus	4:0b4c320bc948	354	rightPWM = speed;
Lauszus	4:0b4c320bc948	355	if (direction == forward) {
Lauszus	4:0b4c320bc948	356	rightA = 0;
Lauszus	4:0b4c320bc948	357	rightB = 1;
Lauszus	4:0b4c320bc948	358	} else if (direction == backward) {
Lauszus	4:0b4c320bc948	359	rightA = 1;
Lauszus	4:0b4c320bc948	360	rightB = 0;
Lauszus	4:0b4c320bc948	361	}
Lauszus	4:0b4c320bc948	362	} else if (motor == both) {
Lauszus	4:0b4c320bc948	363	leftPWM = speed;
Lauszus	4:0b4c320bc948	364	rightPWM = speed;
Lauszus	4:0b4c320bc948	365	if (direction == forward) {
Lauszus	4:0b4c320bc948	366	leftA = 0;
Lauszus	4:0b4c320bc948	367	leftB = 1;
Lauszus	7:f1d24c6119ac	368
Lauszus	4:0b4c320bc948	369	rightA = 0;
Lauszus	4:0b4c320bc948	370	rightB = 1;
Lauszus	4:0b4c320bc948	371	} else if (direction == backward) {
Lauszus	4:0b4c320bc948	372	leftA = 1;
Lauszus	4:0b4c320bc948	373	leftB = 0;
Lauszus	7:f1d24c6119ac	374
Lauszus	4:0b4c320bc948	375	rightA = 1;
Lauszus	4:0b4c320bc948	376	rightB = 0;
Lauszus	4:0b4c320bc948	377	}
Lauszus	4:0b4c320bc948	378	}
Lauszus	4:0b4c320bc948	379	}
Lauszus	4:0b4c320bc948	380	void stop(Motor motor) {
Lauszus	4:0b4c320bc948	381	if (motor == left) {
Lauszus	4:0b4c320bc948	382	leftPWM = 1;
Lauszus	4:0b4c320bc948	383	leftA = 1;
Lauszus	4:0b4c320bc948	384	leftB = 1;
Lauszus	4:0b4c320bc948	385	} else if (motor == right) {
Lauszus	4:0b4c320bc948	386	rightPWM = 1;
Lauszus	4:0b4c320bc948	387	rightA = 1;
Lauszus	4:0b4c320bc948	388	rightB = 1;
Lauszus	4:0b4c320bc948	389	} else if (motor == both) {
Lauszus	4:0b4c320bc948	390	leftPWM = 1;
Lauszus	4:0b4c320bc948	391	leftA = 1;
Lauszus	4:0b4c320bc948	392	leftB = 1;
Lauszus	7:f1d24c6119ac	393
Lauszus	4:0b4c320bc948	394	rightPWM = 1;
Lauszus	4:0b4c320bc948	395	rightA = 1;
Lauszus	4:0b4c320bc948	396	rightB = 1;
Lauszus	4:0b4c320bc948	397	}
Lauszus	0:f5c02b620688	398	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	25 Aug 2016
Imports:	10
Forks:	0
Commits:	11
Dependents:	0
Dependencies:	4
Followers:	1

BalancingRobot.cpp@7:f1d24c6119ac, 2012-04-09 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning