Eurobot_2012_Secondary - Eurobot_2012_Secondary

Users » narshu » Code » Eurobot_2012_Secondary

Shuto Naruse / Mbed 2 deprecated Eurobot_2012_Secondary

Eurobot_2012_Secondary

motors.cpp@0:fbfafa6bf5f9, 2012-04-20 (annotated)

Committer:: narshu
Date:: Fri Apr 20 21:32:24 2012 +0000
Revision:: 0:fbfafa6bf5f9

Who changed what in which revision?

User	Revision	Line number	New contents of line
narshu	0:fbfafa6bf5f9	1	/**********************************************************************
narshu	0:fbfafa6bf5f9	2	* @file motors.cpp
narshu	0:fbfafa6bf5f9	3	* @purpose Eurobot 2012 - Secondary Robot MD25 Interface
narshu	0:fbfafa6bf5f9	4	* @version 0.2
narshu	0:fbfafa6bf5f9	5	* @date 4th April 2012
narshu	0:fbfafa6bf5f9	6	* @author Crispian Poon
narshu	0:fbfafa6bf5f9	7	* @email pooncg@gmail.com
narshu	0:fbfafa6bf5f9	8	______________________________________________________________________
narshu	0:fbfafa6bf5f9	9
narshu	0:fbfafa6bf5f9	10	Setup information:
narshu	0:fbfafa6bf5f9	11	1. Put pull up 2.2k resistors to +3.3v on I2C SCL and SDA
narshu	0:fbfafa6bf5f9	12	2. Connect P28 SDA to MD25 yellow cable, P27 SCL to MD25 blue cable
narshu	0:fbfafa6bf5f9	13
narshu	0:fbfafa6bf5f9	14	**********************************************************************/
narshu	0:fbfafa6bf5f9	15
narshu	0:fbfafa6bf5f9	16	#include "mbed.h"
narshu	0:fbfafa6bf5f9	17	#include "motors.h"
narshu	0:fbfafa6bf5f9	18	#include "globals.h"
narshu	0:fbfafa6bf5f9	19	#include "TSH.h"
narshu	0:fbfafa6bf5f9	20
narshu	0:fbfafa6bf5f9	21	Motors::Motors(TSI2C &i2cin) : i2c(i2cin) {
narshu	0:fbfafa6bf5f9	22
narshu	0:fbfafa6bf5f9	23	}
narshu	0:fbfafa6bf5f9	24
narshu	0:fbfafa6bf5f9	25	//***************************************************************************************
narshu	0:fbfafa6bf5f9	26	//Secondary robot specific instructions
narshu	0:fbfafa6bf5f9	27	//***************************************************************************************
narshu	0:fbfafa6bf5f9	28
narshu	0:fbfafa6bf5f9	29	void Motors::move(int distance, int speed) {
narshu	0:fbfafa6bf5f9	30	//resetEncoders(); TODO use kalman as feedback instead!
narshu	0:fbfafa6bf5f9	31
narshu	0:fbfafa6bf5f9	32	int tempEndEncoder = 0;
narshu	0:fbfafa6bf5f9	33	int startEncoderCount = 0;
narshu	0:fbfafa6bf5f9	34
narshu	0:fbfafa6bf5f9	35	tempEndEncoder = distanceToEncoder(abs(distance));
narshu	0:fbfafa6bf5f9	36	startEncoderCount = getEncoder1();
narshu	0:fbfafa6bf5f9	37
narshu	0:fbfafa6bf5f9	38	setSpeed(getSignOfInt(distance) * speed);
narshu	0:fbfafa6bf5f9	39
narshu	0:fbfafa6bf5f9	40	while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {
narshu	0:fbfafa6bf5f9	41	setSpeed(getSignOfInt(distance) * speed);
narshu	0:fbfafa6bf5f9	42	}
narshu	0:fbfafa6bf5f9	43
narshu	0:fbfafa6bf5f9	44	//resetEncoders();
narshu	0:fbfafa6bf5f9	45	stop();
narshu	0:fbfafa6bf5f9	46	}
narshu	0:fbfafa6bf5f9	47
narshu	0:fbfafa6bf5f9	48	void Motors::turn(int angle, int speed) {
narshu	0:fbfafa6bf5f9	49	//resetEncoders(); TODO use kalman as feedback instead!
narshu	0:fbfafa6bf5f9	50	int tempDistance = int((float(angle) / 360) * float(robotCircumference));
narshu	0:fbfafa6bf5f9	51	int tempEndEncoder = 0;
narshu	0:fbfafa6bf5f9	52	int startEncoderCount = 0;
narshu	0:fbfafa6bf5f9	53
narshu	0:fbfafa6bf5f9	54	tempEndEncoder = distanceToEncoder(abs(tempDistance));
narshu	0:fbfafa6bf5f9	55	startEncoderCount = getEncoder1();
narshu	0:fbfafa6bf5f9	56	setSpeed(getSignOfInt(tempDistance) * speed, -getSignOfInt(tempDistance) * speed);
narshu	0:fbfafa6bf5f9	57
narshu	0:fbfafa6bf5f9	58	while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {
narshu	0:fbfafa6bf5f9	59	setSpeed(getSignOfInt(tempDistance) * speed,-getSignOfInt(tempDistance) * speed);
narshu	0:fbfafa6bf5f9	60
narshu	0:fbfafa6bf5f9	61	}
narshu	0:fbfafa6bf5f9	62
narshu	0:fbfafa6bf5f9	63	//resetEncoders();
narshu	0:fbfafa6bf5f9	64	stop();
narshu	0:fbfafa6bf5f9	65	}
narshu	0:fbfafa6bf5f9	66
narshu	0:fbfafa6bf5f9	67	//***************************************************************************************
narshu	0:fbfafa6bf5f9	68	//Secondary robot specific helper functions
narshu	0:fbfafa6bf5f9	69	//***************************************************************************************
narshu	0:fbfafa6bf5f9	70
narshu	0:fbfafa6bf5f9	71
narshu	0:fbfafa6bf5f9	72	int Motors::getSignOfInt(int direction) {
narshu	0:fbfafa6bf5f9	73
narshu	0:fbfafa6bf5f9	74	direction = (direction < 0);
narshu	0:fbfafa6bf5f9	75
narshu	0:fbfafa6bf5f9	76	switch (direction) {
narshu	0:fbfafa6bf5f9	77	case 1:
narshu	0:fbfafa6bf5f9	78	return -1;
narshu	0:fbfafa6bf5f9	79	case 0:
narshu	0:fbfafa6bf5f9	80	return 1;
narshu	0:fbfafa6bf5f9	81	}
narshu	0:fbfafa6bf5f9	82
narshu	0:fbfafa6bf5f9	83	return 0;
narshu	0:fbfafa6bf5f9	84	}
narshu	0:fbfafa6bf5f9	85
narshu	0:fbfafa6bf5f9	86	// returns distance in mm.
narshu	0:fbfafa6bf5f9	87	float Motors::encoderToDistance(int encoder) {
narshu	0:fbfafa6bf5f9	88	return (float(encoder) / float(encoderRevCount)) * wheelmm;
narshu	0:fbfafa6bf5f9	89	}
narshu	0:fbfafa6bf5f9	90
narshu	0:fbfafa6bf5f9	91	int Motors::distanceToEncoder(float distance) {
narshu	0:fbfafa6bf5f9	92	return int((distance / float(wheelmm)) * encoderRevCount);
narshu	0:fbfafa6bf5f9	93	}
narshu	0:fbfafa6bf5f9	94
narshu	0:fbfafa6bf5f9	95
narshu	0:fbfafa6bf5f9	96	//***************************************************************************************
narshu	0:fbfafa6bf5f9	97	//MD25 instructions
narshu	0:fbfafa6bf5f9	98	//***************************************************************************************
narshu	0:fbfafa6bf5f9	99
narshu	0:fbfafa6bf5f9	100	void Motors::stop() {
narshu	0:fbfafa6bf5f9	101	sendCommand(cmdSetMotor1, 0);
narshu	0:fbfafa6bf5f9	102	sendCommand(cmdSetMotor2, 0);
narshu	0:fbfafa6bf5f9	103	}
narshu	0:fbfafa6bf5f9	104
narshu	0:fbfafa6bf5f9	105	void Motors::setSpeed(int speed) {
narshu	0:fbfafa6bf5f9	106	setMode(1);
narshu	0:fbfafa6bf5f9	107	///sendCommand(cmdByte, 0x30);
narshu	0:fbfafa6bf5f9	108	sendCommand(cmdSetMotor1, speed);
narshu	0:fbfafa6bf5f9	109	sendCommand(cmdSetMotor2, speed);
narshu	0:fbfafa6bf5f9	110	}
narshu	0:fbfafa6bf5f9	111
narshu	0:fbfafa6bf5f9	112	void Motors::setSpeed(int speed1, int speed2) {
narshu	0:fbfafa6bf5f9	113	setMode(1),
narshu	0:fbfafa6bf5f9	114	// sendCommand(cmdByte, 0x30);
narshu	0:fbfafa6bf5f9	115	sendCommand(cmdSetMotor1, speed1);
narshu	0:fbfafa6bf5f9	116	sendCommand(cmdSetMotor2, speed2);
narshu	0:fbfafa6bf5f9	117	}
narshu	0:fbfafa6bf5f9	118
narshu	0:fbfafa6bf5f9	119	void Motors::setMode(int mode) {
narshu	0:fbfafa6bf5f9	120	sendCommand(cmdSetMode, mode);
narshu	0:fbfafa6bf5f9	121	}
narshu	0:fbfafa6bf5f9	122
narshu	0:fbfafa6bf5f9	123	void Motors::resetEncoders() {
narshu	0:fbfafa6bf5f9	124	sendCommand(cmdByte, cmdResetEncoders);
narshu	0:fbfafa6bf5f9	125	}
narshu	0:fbfafa6bf5f9	126
narshu	0:fbfafa6bf5f9	127	int Motors::getEncoder1() {
narshu	0:fbfafa6bf5f9	128	return get4Bytes(cmdGetEncoder1);
narshu	0:fbfafa6bf5f9	129	}
narshu	0:fbfafa6bf5f9	130
narshu	0:fbfafa6bf5f9	131	int Motors::getEncoder2() {
narshu	0:fbfafa6bf5f9	132	return get4Bytes(cmdGetEncoder2);
narshu	0:fbfafa6bf5f9	133	}
narshu	0:fbfafa6bf5f9	134
narshu	0:fbfafa6bf5f9	135	void Motors::disableAcceleration() {
narshu	0:fbfafa6bf5f9	136	sendCommand(cmdByte, cmdDisableAcceleration);
narshu	0:fbfafa6bf5f9	137	}
narshu	0:fbfafa6bf5f9	138
narshu	0:fbfafa6bf5f9	139
narshu	0:fbfafa6bf5f9	140
narshu	0:fbfafa6bf5f9	141	//***************************************************************************************
narshu	0:fbfafa6bf5f9	142	//Abstract MD25 communication methods and functions
narshu	0:fbfafa6bf5f9	143	//***************************************************************************************
narshu	0:fbfafa6bf5f9	144
narshu	0:fbfafa6bf5f9	145	int Motors::get4Bytes(char command) {
narshu	0:fbfafa6bf5f9	146	long tempWord = 0;
narshu	0:fbfafa6bf5f9	147	char cmd[4];
narshu	0:fbfafa6bf5f9	148
narshu	0:fbfafa6bf5f9	149	//i2c request
narshu	0:fbfafa6bf5f9	150	sendCommand(command);
narshu	0:fbfafa6bf5f9	151
narshu	0:fbfafa6bf5f9	152	//i2c read data back
narshu	0:fbfafa6bf5f9	153	i2c.read(md25Address, cmd, 4);// Request 4 bytes from MD25
narshu	0:fbfafa6bf5f9	154
narshu	0:fbfafa6bf5f9	155	//FIXED 22FEB2012 CRISPIAN Taken 0.07 delay off.
narshu	0:fbfafa6bf5f9	156
narshu	0:fbfafa6bf5f9	157	//First byte is largest, shift 4 bytes into tempWord
narshu	0:fbfafa6bf5f9	158	tempWord += cmd[0] << 24;
narshu	0:fbfafa6bf5f9	159	tempWord += cmd[1] << 16;
narshu	0:fbfafa6bf5f9	160	tempWord += cmd[2] << 8;
narshu	0:fbfafa6bf5f9	161	tempWord += cmd[3] ;
narshu	0:fbfafa6bf5f9	162
narshu	0:fbfafa6bf5f9	163	return tempWord;
narshu	0:fbfafa6bf5f9	164	}
narshu	0:fbfafa6bf5f9	165
narshu	0:fbfafa6bf5f9	166	void Motors::sendCommand(char command) {
narshu	0:fbfafa6bf5f9	167	char buffer[1];
narshu	0:fbfafa6bf5f9	168	buffer[0] = command;
narshu	0:fbfafa6bf5f9	169	i2c.write(md25Address, &buffer[0], 1);
narshu	0:fbfafa6bf5f9	170	}
narshu	0:fbfafa6bf5f9	171
narshu	0:fbfafa6bf5f9	172	void Motors::sendCommand(char command1, char command2 ) {
narshu	0:fbfafa6bf5f9	173
narshu	0:fbfafa6bf5f9	174	char buffer[2];
narshu	0:fbfafa6bf5f9	175	buffer[0] = command1;
narshu	0:fbfafa6bf5f9	176	buffer[1] = command2;
narshu	0:fbfafa6bf5f9	177
narshu	0:fbfafa6bf5f9	178	i2c.write(md25Address, &buffer[0], 2);
narshu	0:fbfafa6bf5f9	179	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	20 Apr 2012
Imports:	2
Forks:	1
Commits:	1
Dependents:	0
Dependencies:	2
Followers:	1

motors.cpp@0:fbfafa6bf5f9, 2012-04-20 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning