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@2:caec5534774d, 2012-02-16 (annotated)

Committer:: Lauszus
Date:: Thu Feb 16 20:25:52 2012 +0000
Revision:: 2:caec5534774d
Parent:: 1:01295228342f
Child:: 3:c3963f37d597

Who changed what in which revision?

User	Revision	Line number	New contents of line
Lauszus	2:caec5534774d	1	/*
Lauszus	1:01295228342f	2	* The code is released under the GNU General Public License.
Lauszus	1:01295228342f	3	* Developed by Kristian Lauszus
Lauszus	1:01295228342f	4	* This is the algorithm for my balancing robot/segway.
Lauszus	1:01295228342f	5	* It is controlled by a PS3 Controller via bluetooth.
Lauszus	1:01295228342f	6	* The remote control code can be found at my other github repository: https://github.com/TKJElectronics/BalancingRobot
Lauszus	1:01295228342f	7	* For details, see http://blog.tkjelectronics.dk
Lauszus	1:01295228342f	8	*/
Lauszus	1:01295228342f	9
Lauszus	0:f5c02b620688	10	#include "BalancingRobot.h"
Lauszus	0:f5c02b620688	11
Lauszus	0:f5c02b620688	12	/* Serial communication */
Lauszus	0:f5c02b620688	13	Serial xbee(p13,p14); // For wireless debugging
Lauszus	0:f5c02b620688	14	Serial ps3(p9,p10); // For remote control
Lauszus	0:f5c02b620688	15	Serial debug(USBTX, USBRX); // USB
Lauszus	0:f5c02b620688	16
Lauszus	0:f5c02b620688	17	/* Timer instance */
Lauszus	0:f5c02b620688	18	Timer t;
Lauszus	0:f5c02b620688	19
Lauszus	0:f5c02b620688	20	int main() {
Lauszus	0:f5c02b620688	21	xbee.baud(115200);
Lauszus	0:f5c02b620688	22	ps3.baud(115200);
Lauszus	0:f5c02b620688	23	debug.baud(115200);
Lauszus	0:f5c02b620688	24
Lauszus	0:f5c02b620688	25	leftPWM.period(0.00005); //The motor driver can handle pwm values up to 20kHz (1/20000=0.00005)
Lauszus	0:f5c02b620688	26	rightPWM.period(0.00005);
Lauszus	0:f5c02b620688	27
Lauszus	0:f5c02b620688	28	calibrateSensors(); // Calibrate the gyro and accelerometer relative to ground
Lauszus	0:f5c02b620688	29
Lauszus	0:f5c02b620688	30	xbee.printf("Initialized\n");
Lauszus	2:caec5534774d	31	processing(); // Send output to processing application
Lauszus	0:f5c02b620688	32
Lauszus	0:f5c02b620688	33	// Start timing
Lauszus	0:f5c02b620688	34	t.start();
Lauszus	0:f5c02b620688	35	loopStartTime = t.read_us();
Lauszus	0:f5c02b620688	36	timer = loopStartTime;
Lauszus	0:f5c02b620688	37
Lauszus	0:f5c02b620688	38	while (1) {
Lauszus	0:f5c02b620688	39	// 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	0:f5c02b620688	40	accYangle = getAccY();
Lauszus	0:f5c02b620688	41	gyroYrate = getGyroYrate();
Lauszus	0:f5c02b620688	42
Lauszus	0:f5c02b620688	43	pitch = kalman(accYangle, gyroYrate, t.read_us() - timer); // calculate the angle using a Kalman filter
Lauszus	0:f5c02b620688	44	timer = t.read_us();
Lauszus	0:f5c02b620688	45
Lauszus	0:f5c02b620688	46	if (ps3.readable()) // Check if there's any incoming data
Lauszus	2:caec5534774d	47	receivePS3();
Lauszus	2:caec5534774d	48	if (xbee.readable()) // For setting the PID values
Lauszus	2:caec5534774d	49	receiveXbee();
Lauszus	0:f5c02b620688	50
Lauszus	0:f5c02b620688	51	//debug.printf("Pitch: %f, accYangle: %f\n",pitch,accYangle);
Lauszus	0:f5c02b620688	52
Lauszus	1:01295228342f	53	if (pitch < 75 \|\| pitch > 105) // Stop if falling or laying down
Lauszus	0:f5c02b620688	54	stopAndReset();
Lauszus	0:f5c02b620688	55	else
Lauszus	2:caec5534774d	56	PID(targetAngle,targetOffset);
Lauszus	0:f5c02b620688	57
Lauszus	0:f5c02b620688	58	/* Used a time fixed loop */
Lauszus	0:f5c02b620688	59	lastLoopUsefulTime = t.read_us() - loopStartTime;
Lauszus	0:f5c02b620688	60	if (lastLoopUsefulTime < STD_LOOP_TIME)
Lauszus	0:f5c02b620688	61	wait_us(STD_LOOP_TIME - lastLoopUsefulTime);
Lauszus	0:f5c02b620688	62	lastLoopTime = t.read_us() - loopStartTime; // only used for debugging
Lauszus	0:f5c02b620688	63	loopStartTime = t.read_us();
Lauszus	0:f5c02b620688	64
Lauszus	0:f5c02b620688	65	//debug.printf("%i,%i\n",lastLoopUsefulTime,lastLoopTime);
Lauszus	0:f5c02b620688	66	}
Lauszus	0:f5c02b620688	67	}
Lauszus	2:caec5534774d	68	void receivePS3() {
Lauszus	2:caec5534774d	69	char input[16]; // The serial buffer is only 16 characters
Lauszus	2:caec5534774d	70	int i = 0;
Lauszus	2:caec5534774d	71	while (1) {
Lauszus	2:caec5534774d	72	input[i] = ps3.getc(); // keep reading until it reads a semicolon
Lauszus	2:caec5534774d	73	if (input[i] == ';')
Lauszus	1:01295228342f	74	break;
Lauszus	2:caec5534774d	75	i++;
Lauszus	2:caec5534774d	76	}
Lauszus	0:f5c02b620688	77	//debug.printf("Input: %s\n",input);
Lauszus	0:f5c02b620688	78
Lauszus	2:caec5534774d	79	// Set all false
Lauszus	2:caec5534774d	80	steerForward = false;
Lauszus	2:caec5534774d	81	steerBackward = false;
Lauszus	2:caec5534774d	82	steerLeft = false;
Lauszus	2:caec5534774d	83	steerRotateLeft = false;
Lauszus	2:caec5534774d	84	steerRight = false;
Lauszus	2:caec5534774d	85	steerRotateRight = false;
Lauszus	2:caec5534774d	86
Lauszus	0:f5c02b620688	87	/* For remote control */
Lauszus	0:f5c02b620688	88	if (input[0] == 'F') { // Forward
Lauszus	2:caec5534774d	89	strtok(input, ","); // Ignore 'F'
Lauszus	2:caec5534774d	90	targetOffset = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
Lauszus	2:caec5534774d	91	xbee.printf("%f\n",targetOffset); // Print targetOffset for debugging
Lauszus	0:f5c02b620688	92	steerForward = true;
Lauszus	0:f5c02b620688	93	} else if (input[0] == 'B') { // Backward
Lauszus	2:caec5534774d	94	strtok(input, ","); // Ignore 'B'
Lauszus	2:caec5534774d	95	targetOffset = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
Lauszus	2:caec5534774d	96	xbee.printf("%f\n",targetOffset); // Print targetOffset for debugging
Lauszus	0:f5c02b620688	97	steerBackward = true;
Lauszus	0:f5c02b620688	98	} else if (input[0] == 'L') { // Left
Lauszus	2:caec5534774d	99	if (input[1] == 'R') // Left Rotate
Lauszus	0:f5c02b620688	100	steerRotateLeft = true;
Lauszus	2:caec5534774d	101	else
Lauszus	0:f5c02b620688	102	steerLeft = true;
Lauszus	0:f5c02b620688	103	} else if (input[0] == 'R') { // Right
Lauszus	2:caec5534774d	104	if (input[1] == 'R') // Right Rotate
Lauszus	0:f5c02b620688	105	steerRotateRight = true;
Lauszus	2:caec5534774d	106	else
Lauszus	0:f5c02b620688	107	steerRight = true;
Lauszus	0:f5c02b620688	108	} else if (input[0] == 'S') { // Stop
Lauszus	2:caec5534774d	109	// Everything is allready false
Lauszus	0:f5c02b620688	110	}
Lauszus	2:caec5534774d	111
Lauszus	0:f5c02b620688	112	else if (input[0] == 'T') { // Set the target angle
Lauszus	0:f5c02b620688	113	strtok(input, ","); // Ignore 'T'
Lauszus	1:01295228342f	114	targetAngle = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
Lauszus	0:f5c02b620688	115	xbee.printf("%f\n",targetAngle); // Print targetAngle for debugging
Lauszus	0:f5c02b620688	116	} else if (input[0] == 'A') { // Abort
Lauszus	0:f5c02b620688	117	stopAndReset();
Lauszus	0:f5c02b620688	118	while (ps3.getc() != 'C'); // Wait until continue is send
Lauszus	0:f5c02b620688	119	}
Lauszus	0:f5c02b620688	120	}
Lauszus	2:caec5534774d	121	void receiveXbee() {
Lauszus	2:caec5534774d	122	char input[16]; // The serial buffer is only 16 characters
Lauszus	2:caec5534774d	123	int i = 0;
Lauszus	2:caec5534774d	124	while (1) { // keep reading until it reads a semicolon
Lauszus	2:caec5534774d	125	input[i] = xbee.getc();
Lauszus	2:caec5534774d	126	if (input[i] == ';')
Lauszus	2:caec5534774d	127	break;
Lauszus	2:caec5534774d	128	i++;
Lauszus	2:caec5534774d	129	}
Lauszus	2:caec5534774d	130	//debug.printf("Input: %s\n",input);
Lauszus	2:caec5534774d	131
Lauszus	2:caec5534774d	132	if (input[0] == 'T') { // Set the target angle
Lauszus	2:caec5534774d	133	strtok(input, ","); // Ignore 'T'
Lauszus	2:caec5534774d	134	targetAngle = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
Lauszus	2:caec5534774d	135	} else if (input[0] == 'P') {
Lauszus	2:caec5534774d	136	strtok(input, ",");//Ignore 'P'
Lauszus	2:caec5534774d	137	Kp = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
Lauszus	2:caec5534774d	138	} else if (input[0] == 'I') {
Lauszus	2:caec5534774d	139	strtok(input, ",");//Ignore 'I'
Lauszus	2:caec5534774d	140	Ki = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
Lauszus	2:caec5534774d	141	} else if (input[0] == 'D') {
Lauszus	2:caec5534774d	142	strtok(input, ",");//Ignore 'D'
Lauszus	2:caec5534774d	143	Kd = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
Lauszus	2:caec5534774d	144	} else if (input[0] == 'A') { // Abort
Lauszus	2:caec5534774d	145	stopAndReset();
Lauszus	2:caec5534774d	146	while (xbee.getc() != 'C'); // Wait until continue is send
Lauszus	2:caec5534774d	147	} else if (input[0] == 'G') // The processing application sends this at startup
Lauszus	2:caec5534774d	148	processing(); // Send output to processing application
Lauszus	2:caec5534774d	149	}
Lauszus	2:caec5534774d	150	void processing() {
Lauszus	2:caec5534774d	151	/* Send output to processing application */
Lauszus	2:caec5534774d	152	xbee.printf("Processing,%5.2f,%5.2f,%5.2f,%5.2f\n",Kp,Ki,Kd,targetAngle);
Lauszus	2:caec5534774d	153	}
Lauszus	2:caec5534774d	154	void PID(double restAngle, double offset) {
Lauszus	0:f5c02b620688	155	if (steerForward)
Lauszus	2:caec5534774d	156	restAngle -= offset;
Lauszus	0:f5c02b620688	157	else if (steerBackward)
Lauszus	2:caec5534774d	158	restAngle += offset;
Lauszus	0:f5c02b620688	159
Lauszus	0:f5c02b620688	160	double error = (restAngle - pitch)/100;
Lauszus	0:f5c02b620688	161	double pTerm = Kp * error;
Lauszus	0:f5c02b620688	162	iTerm += Ki * error;
Lauszus	0:f5c02b620688	163	double dTerm = Kd * (error - lastError);
Lauszus	0:f5c02b620688	164	lastError = error;
Lauszus	0:f5c02b620688	165
Lauszus	0:f5c02b620688	166	double PIDValue = pTerm + iTerm + dTerm;
Lauszus	0:f5c02b620688	167
Lauszus	0:f5c02b620688	168	//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	0:f5c02b620688	169
Lauszus	0:f5c02b620688	170	double PIDLeft;
Lauszus	0:f5c02b620688	171	double PIDRight;
Lauszus	0:f5c02b620688	172	if (steerLeft) {
Lauszus	0:f5c02b620688	173	PIDLeft = PIDValue-(0.1);
Lauszus	0:f5c02b620688	174	PIDRight = PIDValue+(0.1);
Lauszus	0:f5c02b620688	175	} else if (steerRotateLeft) {
Lauszus	0:f5c02b620688	176	PIDLeft = PIDValue-(0.2);
Lauszus	0:f5c02b620688	177	PIDRight = PIDValue+(0.2);
Lauszus	0:f5c02b620688	178	} else if (steerRight) {
Lauszus	0:f5c02b620688	179	PIDLeft = PIDValue-(-0.1);
Lauszus	0:f5c02b620688	180	PIDRight = PIDValue+(-0.1);
Lauszus	0:f5c02b620688	181	} else if (steerRotateRight) {
Lauszus	0:f5c02b620688	182	PIDLeft = PIDValue-(-0.2);
Lauszus	0:f5c02b620688	183	PIDRight = PIDValue+(-0.2);
Lauszus	0:f5c02b620688	184	} else {
Lauszus	0:f5c02b620688	185	PIDLeft = PIDValue;
Lauszus	0:f5c02b620688	186	PIDRight = PIDValue;
Lauszus	0:f5c02b620688	187	}
Lauszus	0:f5c02b620688	188	PIDLeft *= 0.9; // compensate for difference in the motors
Lauszus	0:f5c02b620688	189
Lauszus	0:f5c02b620688	190	//Set PWM Values
Lauszus	0:f5c02b620688	191	if (PIDLeft >= 0)
Lauszus	0:f5c02b620688	192	move(left, forward, PIDLeft);
Lauszus	0:f5c02b620688	193	else
Lauszus	0:f5c02b620688	194	move(left, backward, PIDLeft * -1);
Lauszus	0:f5c02b620688	195	if (PIDRight >= 0)
Lauszus	0:f5c02b620688	196	move(right, forward, PIDRight);
Lauszus	0:f5c02b620688	197	else
Lauszus	0:f5c02b620688	198	move(right, backward, PIDRight * -1);
Lauszus	0:f5c02b620688	199	}
Lauszus	2:caec5534774d	200	void stopAndReset() {
Lauszus	2:caec5534774d	201	stop(both);
Lauszus	2:caec5534774d	202	lastError = 0;
Lauszus	2:caec5534774d	203	iTerm = 0;
Lauszus	2:caec5534774d	204	}
Lauszus	0:f5c02b620688	205	double kalman(double newAngle, double newRate, double dtime) {
Lauszus	0:f5c02b620688	206	// 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	0:f5c02b620688	207	// with slightly modifications by Kristian Lauszus
Lauszus	0:f5c02b620688	208	dt = dtime / 1000000; // Convert from microseconds to seconds
Lauszus	0:f5c02b620688	209
Lauszus	0:f5c02b620688	210	// Discrete Kalman filter time update equations - Time Update ("Predict")
Lauszus	0:f5c02b620688	211	// Update xhat - Project the state ahead
Lauszus	0:f5c02b620688	212	angle += dt * (newRate - bias);
Lauszus	0:f5c02b620688	213
Lauszus	0:f5c02b620688	214	// Update estimation error covariance - Project the error covariance ahead
Lauszus	0:f5c02b620688	215	P_00 += -dt * (P_10 + P_01) + Q_angle * dt;
Lauszus	0:f5c02b620688	216	P_01 += -dt * P_11;
Lauszus	0:f5c02b620688	217	P_10 += -dt * P_11;
Lauszus	0:f5c02b620688	218	P_11 += +Q_gyro * dt;
Lauszus	0:f5c02b620688	219
Lauszus	0:f5c02b620688	220	// Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
Lauszus	0:f5c02b620688	221	// Calculate Kalman gain - Compute the Kalman gain
Lauszus	0:f5c02b620688	222	S = P_00 + R_angle;
Lauszus	0:f5c02b620688	223	K_0 = P_00 / S;
Lauszus	0:f5c02b620688	224	K_1 = P_10 / S;
Lauszus	0:f5c02b620688	225
Lauszus	0:f5c02b620688	226	// Calculate angle and resting rate - Update estimate with measurement zk
Lauszus	0:f5c02b620688	227	y = newAngle - angle;
Lauszus	0:f5c02b620688	228	angle += K_0 * y;
Lauszus	0:f5c02b620688	229	bias += K_1 * y;
Lauszus	0:f5c02b620688	230
Lauszus	0:f5c02b620688	231	// Calculate estimation error covariance - Update the error covariance
Lauszus	0:f5c02b620688	232	P_00 -= K_0 * P_00;
Lauszus	0:f5c02b620688	233	P_01 -= K_0 * P_01;
Lauszus	0:f5c02b620688	234	P_10 -= K_1 * P_00;
Lauszus	0:f5c02b620688	235	P_11 -= K_1 * P_01;
Lauszus	0:f5c02b620688	236
Lauszus	0:f5c02b620688	237	return angle;
Lauszus	0:f5c02b620688	238	}
Lauszus	0:f5c02b620688	239	double getGyroYrate() {
Lauszus	0:f5c02b620688	240	// (gyroAdc-gyroZero)/Sensitivity (In quids) - Sensitivity = 0.00333/3.3*4095=4.132227273
Lauszus	0:f5c02b620688	241	double gyroRate = -(((gyroY.read()*4095) - zeroValues[0]) / 4.132227273);
Lauszus	0:f5c02b620688	242	return gyroRate;
Lauszus	0:f5c02b620688	243	}
Lauszus	0:f5c02b620688	244	double getAccY() {
Lauszus	0:f5c02b620688	245	// (accAdc-accZero)/Sensitivity (In quids) - Sensitivity = 0.33/3.3*4095=409.5
Lauszus	0:f5c02b620688	246	double accXval = ((accX.read()*4095) - zeroValues[1]) / 409.5;
Lauszus	0:f5c02b620688	247	double accYval = ((accY.read()*4095) - zeroValues[2]) / 409.5;
Lauszus	0:f5c02b620688	248	accYval--;//-1g when lying down
Lauszus	0:f5c02b620688	249	double accZval = ((accZ.read()*4095) - zeroValues[3]) / 409.5;
Lauszus	0:f5c02b620688	250
Lauszus	0:f5c02b620688	251	double R = sqrt(pow(accXval, 2) + pow(accYval, 2) + pow(accZval, 2)); // Calculate the force vector
Lauszus	0:f5c02b620688	252	double angleY = acos(accYval / R) * RAD_TO_DEG;
Lauszus	0:f5c02b620688	253
Lauszus	0:f5c02b620688	254	return angleY;
Lauszus	0:f5c02b620688	255	}
Lauszus	0:f5c02b620688	256	void calibrateSensors() {
Lauszus	0:f5c02b620688	257	onboardLED1 = 1;
Lauszus	0:f5c02b620688	258	onboardLED2 = 1;
Lauszus	0:f5c02b620688	259	onboardLED3 = 1;
Lauszus	0:f5c02b620688	260	onboardLED4 = 1;
Lauszus	0:f5c02b620688	261
Lauszus	0:f5c02b620688	262	double adc[4];
Lauszus	0:f5c02b620688	263	for (uint8_t i = 0; i < 100; i++) { // Take the average of 100 readings
Lauszus	0:f5c02b620688	264	adc[0] += gyroY.read()*4095;
Lauszus	0:f5c02b620688	265	adc[1] += accX.read()*4095;
Lauszus	0:f5c02b620688	266	adc[2] += accY.read()*4095;
Lauszus	0:f5c02b620688	267	adc[3] += accZ.read()*4095;
Lauszus	0:f5c02b620688	268	wait_ms(10);
Lauszus	0:f5c02b620688	269	}
Lauszus	0:f5c02b620688	270	zeroValues[0] = adc[0] / 100; // Gyro X-axis
Lauszus	0:f5c02b620688	271	zeroValues[1] = adc[1] / 100; // Accelerometer X-axis
Lauszus	0:f5c02b620688	272	zeroValues[2] = adc[2] / 100; // Accelerometer Y-axis
Lauszus	0:f5c02b620688	273	zeroValues[3] = adc[3] / 100; // Accelerometer Z-axis
Lauszus	0:f5c02b620688	274
Lauszus	0:f5c02b620688	275	onboardLED1 = 0;
Lauszus	0:f5c02b620688	276	onboardLED2 = 0;
Lauszus	0:f5c02b620688	277	onboardLED3 = 0;
Lauszus	0:f5c02b620688	278	onboardLED4 = 0;
Lauszus	0:f5c02b620688	279	}
Lauszus	0:f5c02b620688	280	void move(Motor motor, Direction direction, float speed) { // speed is a value in percentage (0.0f to 1.0f)
Lauszus	0:f5c02b620688	281	if (motor == right) {
Lauszus	0:f5c02b620688	282	leftPWM = speed;
Lauszus	0:f5c02b620688	283	if (direction == forward) {
Lauszus	0:f5c02b620688	284	leftA = 0;
Lauszus	0:f5c02b620688	285	leftB = 1;
Lauszus	0:f5c02b620688	286	} else if (direction == backward) {
Lauszus	0:f5c02b620688	287	leftA = 1;
Lauszus	0:f5c02b620688	288	leftB = 0;
Lauszus	0:f5c02b620688	289	}
Lauszus	0:f5c02b620688	290	} else if (motor == left) {
Lauszus	0:f5c02b620688	291	rightPWM = speed;
Lauszus	0:f5c02b620688	292	if (direction == forward) {
Lauszus	0:f5c02b620688	293	rightA = 0;
Lauszus	0:f5c02b620688	294	rightB = 1;
Lauszus	0:f5c02b620688	295	} else if (direction == backward) {
Lauszus	0:f5c02b620688	296	rightA = 1;
Lauszus	0:f5c02b620688	297	rightB = 0;
Lauszus	0:f5c02b620688	298	}
Lauszus	0:f5c02b620688	299	} else if (motor == both) {
Lauszus	0:f5c02b620688	300	leftPWM = speed;
Lauszus	0:f5c02b620688	301	rightPWM = speed;
Lauszus	0:f5c02b620688	302	if (direction == forward) {
Lauszus	0:f5c02b620688	303	leftA = 0;
Lauszus	0:f5c02b620688	304	leftB = 1;
Lauszus	0:f5c02b620688	305
Lauszus	0:f5c02b620688	306	rightA = 0;
Lauszus	0:f5c02b620688	307	rightB = 1;
Lauszus	0:f5c02b620688	308	} else if (direction == backward) {
Lauszus	0:f5c02b620688	309	leftA = 1;
Lauszus	0:f5c02b620688	310	leftB = 0;
Lauszus	0:f5c02b620688	311
Lauszus	0:f5c02b620688	312	rightA = 1;
Lauszus	0:f5c02b620688	313	rightB = 0;
Lauszus	0:f5c02b620688	314	}
Lauszus	0:f5c02b620688	315	}
Lauszus	0:f5c02b620688	316	}
Lauszus	0:f5c02b620688	317	void stop(Motor motor) {
Lauszus	0:f5c02b620688	318	if (motor == left) {
Lauszus	0:f5c02b620688	319	leftPWM = 1;
Lauszus	0:f5c02b620688	320	leftA = 1;
Lauszus	0:f5c02b620688	321	leftB = 1;
Lauszus	0:f5c02b620688	322	} else if (motor == right) {
Lauszus	0:f5c02b620688	323	rightPWM = 1;
Lauszus	0:f5c02b620688	324	rightA = 1;
Lauszus	0:f5c02b620688	325	rightB = 1;
Lauszus	0:f5c02b620688	326	} else if (motor == both) {
Lauszus	0:f5c02b620688	327	leftPWM = 1;
Lauszus	0:f5c02b620688	328	leftA = 1;
Lauszus	0:f5c02b620688	329	leftB = 1;
Lauszus	0:f5c02b620688	330
Lauszus	0:f5c02b620688	331	rightPWM = 1;
Lauszus	0:f5c02b620688	332	rightA = 1;
Lauszus	0:f5c02b620688	333	rightB = 1;
Lauszus	0:f5c02b620688	334	}
Lauszus	0:f5c02b620688	335	}

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@2:caec5534774d, 2012-02-16 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning