scmRTOS_test - Program to control an accelerometer, motors and a…

Users » jberry » Code » scmRTOS_test

Greg Kuhn / Mbed 2 deprecated scmRTOS_test

Program to control an accelerometer, motors and a rangefinder using the ScmRTOS ported to mbed. (Work in progress and buggy)

Dependencies: mbed

Hexacopter/Motor_control.h@0:9b057566f9ee, 2010-11-01 (annotated)

Committer:: jberry
Date:: Mon Nov 01 20:39:01 2010 +0000
Revision:: 0:9b057566f9ee

Who changed what in which revision?

User	Revision	Line number	New contents of line
jberry	0:9b057566f9ee	1	#pragma once
jberry	0:9b057566f9ee	2	#ifndef MOTOR_CONTROL_H
jberry	0:9b057566f9ee	3	#define MOTOR_CONTROL_H
jberry	0:9b057566f9ee	4	#include <cmath>
jberry	0:9b057566f9ee	5	#include <processes.h>
jberry	0:9b057566f9ee	6
jberry	0:9b057566f9ee	7
jberry	0:9b057566f9ee	8	//Motor_controller M_cont_process;
jberry	0:9b057566f9ee	9	enum direction{CLOCKWISE,ANTICLOCKWISE};
jberry	0:9b057566f9ee	10	//PwmOut Out1(p23); //motor1
jberry	0:9b057566f9ee	11	//PwmOut Out2(p24);
jberry	0:9b057566f9ee	12	//PwmOut Out3(p25);
jberry	0:9b057566f9ee	13	//PwmOut Out4(p26);
jberry	0:9b057566f9ee	14
jberry	0:9b057566f9ee	15	class motor_control
jberry	0:9b057566f9ee	16	{
jberry	0:9b057566f9ee	17	public:
jberry	0:9b057566f9ee	18	motor_control(PinName OUT1, PinName OUT2)
jberry	0:9b057566f9ee	19	:Out1(OUT1),Out2(OUT2),Output(&Out1)
jberry	0:9b057566f9ee	20	{
jberry	0:9b057566f9ee	21	//Output = &Out1;
jberry	0:9b057566f9ee	22	set_direction(CLOCKWISE);
jberry	0:9b057566f9ee	23	SPEED = 0.0;
jberry	0:9b057566f9ee	24	Out1.pulsewidth_ms(1); // pulsewidth should be 1ms
jberry	0:9b057566f9ee	25	Out1.write(0.0); //set initial duty cycle to 0 (i.e. off)
jberry	0:9b057566f9ee	26	Out2.write(0.0); //set initial duty cycle to 0 (i.e. off)
jberry	0:9b057566f9ee	27
jberry	0:9b057566f9ee	28
jberry	0:9b057566f9ee	29
jberry	0:9b057566f9ee	30
jberry	0:9b057566f9ee	31	}
jberry	0:9b057566f9ee	32
jberry	0:9b057566f9ee	33	void set_direction(direction D)
jberry	0:9b057566f9ee	34	{
jberry	0:9b057566f9ee	35	if( D == CLOCKWISE )
jberry	0:9b057566f9ee	36	{
jberry	0:9b057566f9ee	37	*Output = 0;
jberry	0:9b057566f9ee	38	Output = &Out2;
jberry	0:9b057566f9ee	39	}
jberry	0:9b057566f9ee	40	else
jberry	0:9b057566f9ee	41	{
jberry	0:9b057566f9ee	42	*Output = 0;
jberry	0:9b057566f9ee	43	Output = &Out1;
jberry	0:9b057566f9ee	44	}
jberry	0:9b057566f9ee	45	DIR = D;
jberry	0:9b057566f9ee	46	//set_speed(get_speed());
jberry	0:9b057566f9ee	47	}
jberry	0:9b057566f9ee	48
jberry	0:9b057566f9ee	49	direction get_direction()
jberry	0:9b057566f9ee	50	{
jberry	0:9b057566f9ee	51	return DIR;
jberry	0:9b057566f9ee	52	}
jberry	0:9b057566f9ee	53
jberry	0:9b057566f9ee	54	void set_speed(float speed)
jberry	0:9b057566f9ee	55	{
jberry	0:9b057566f9ee	56	SPEED = speed;
jberry	0:9b057566f9ee	57	if(speed >= -1 && speed < 0)
jberry	0:9b057566f9ee	58	{
jberry	0:9b057566f9ee	59	if(get_direction() == CLOCKWISE) //ALLOW SOME TIME for the motor to change direction
jberry	0:9b057566f9ee	60	{
jberry	0:9b057566f9ee	61	*Output = 0;
jberry	0:9b057566f9ee	62	//wait_ms(500);
jberry	0:9b057566f9ee	63	}
jberry	0:9b057566f9ee	64
jberry	0:9b057566f9ee	65	set_direction(ANTICLOCKWISE);
jberry	0:9b057566f9ee	66	*Output = speed;
jberry	0:9b057566f9ee	67	}
jberry	0:9b057566f9ee	68	else if(speed >= 0 && speed <= 1)
jberry	0:9b057566f9ee	69	{
jberry	0:9b057566f9ee	70	if(get_direction() == CLOCKWISE) //ALLOW SOME TIME for the motor to change direction
jberry	0:9b057566f9ee	71	{
jberry	0:9b057566f9ee	72	*Output = 0;
jberry	0:9b057566f9ee	73	//wait_ms(500);
jberry	0:9b057566f9ee	74	}
jberry	0:9b057566f9ee	75
jberry	0:9b057566f9ee	76	set_direction(CLOCKWISE);
jberry	0:9b057566f9ee	77	*Output = speed;
jberry	0:9b057566f9ee	78	}
jberry	0:9b057566f9ee	79
jberry	0:9b057566f9ee	80	}
jberry	0:9b057566f9ee	81	float get_speed()
jberry	0:9b057566f9ee	82	{
jberry	0:9b057566f9ee	83	return SPEED;
jberry	0:9b057566f9ee	84	}
jberry	0:9b057566f9ee	85
jberry	0:9b057566f9ee	86	void speed_inc()
jberry	0:9b057566f9ee	87	{
jberry	0:9b057566f9ee	88	if( (get_speed() + 0.1) <= 1)
jberry	0:9b057566f9ee	89	set_speed(SPEED + 0.1);
jberry	0:9b057566f9ee	90	else if( get_speed() + 0.1 > 1)
jberry	0:9b057566f9ee	91	set_speed(1);
jberry	0:9b057566f9ee	92	}
jberry	0:9b057566f9ee	93
jberry	0:9b057566f9ee	94	void speed_dec()
jberry	0:9b057566f9ee	95	{
jberry	0:9b057566f9ee	96	if( (get_speed() - 0.1) >= 0)
jberry	0:9b057566f9ee	97	set_speed(SPEED - 0.1);
jberry	0:9b057566f9ee	98	else if( get_speed() - 0.1 < 0)
jberry	0:9b057566f9ee	99	set_speed(0);
jberry	0:9b057566f9ee	100
jberry	0:9b057566f9ee	101	}
jberry	0:9b057566f9ee	102
jberry	0:9b057566f9ee	103	void change_direction()
jberry	0:9b057566f9ee	104	{
jberry	0:9b057566f9ee	105	if (get_direction() == CLOCKWISE)
jberry	0:9b057566f9ee	106	set_direction(ANTICLOCKWISE);
jberry	0:9b057566f9ee	107	else
jberry	0:9b057566f9ee	108	set_direction(CLOCKWISE);
jberry	0:9b057566f9ee	109	}
jberry	0:9b057566f9ee	110
jberry	0:9b057566f9ee	111
jberry	0:9b057566f9ee	112	private:
jberry	0:9b057566f9ee	113	//int PIN1,PIN2;
jberry	0:9b057566f9ee	114	direction DIR;
jberry	0:9b057566f9ee	115	PwmOut Out1;
jberry	0:9b057566f9ee	116	PwmOut Out2;
jberry	0:9b057566f9ee	117	PwmOut* Output;
jberry	0:9b057566f9ee	118
jberry	0:9b057566f9ee	119
jberry	0:9b057566f9ee	120	float SPEED;
jberry	0:9b057566f9ee	121
jberry	0:9b057566f9ee	122	};
jberry	0:9b057566f9ee	123
jberry	0:9b057566f9ee	124
jberry	0:9b057566f9ee	125	char MOT_Return_chars[100]; //defined in accelerometer.h
jberry	0:9b057566f9ee	126	char Motor_command[100] = "";
jberry	0:9b057566f9ee	127	byte Motor_number = 0;
jberry	0:9b057566f9ee	128
jberry	0:9b057566f9ee	129	extern BusOut leds;
jberry	0:9b057566f9ee	130	template<> OS_PROCESS void Motor_controller::Exec() //motor control handling process
jberry	0:9b057566f9ee	131	{
jberry	0:9b057566f9ee	132	Motor_command[99] = '\0';
jberry	0:9b057566f9ee	133	motor_control Motor1(p23,p24);
jberry	0:9b057566f9ee	134	motor_control Motor2(p25,p26);
jberry	0:9b057566f9ee	135	motor_control *M = &Motor1;
jberry	0:9b057566f9ee	136	float motor_percentage = -1, motor_direction = 0.5, motor_strength = 0; //motor direction is a float from 0-1 which indicates the relative strength of one motor to the other the strength is the PWM output of the strongest motor as dictated by direction
jberry	0:9b057566f9ee	137	direction PREV_DIR = CLOCKWISE;
jberry	0:9b057566f9ee	138
jberry	0:9b057566f9ee	139
jberry	0:9b057566f9ee	140
jberry	0:9b057566f9ee	141
jberry	0:9b057566f9ee	142	char* pEND, *pEND2;
jberry	0:9b057566f9ee	143
jberry	0:9b057566f9ee	144	//MOT_channel
jberry	0:9b057566f9ee	145	for (;;)
jberry	0:9b057566f9ee	146	{
jberry	0:9b057566f9ee	147
jberry	0:9b057566f9ee	148	//OS::message<byte> MOTOR_message;
jberry	0:9b057566f9ee	149	for(byte i = 0; i<100; i++)
jberry	0:9b057566f9ee	150	{
jberry	0:9b057566f9ee	151	MOT_channel.pop(Motor_command[i]); //if data is available, pop, else wait
jberry	0:9b057566f9ee	152	leds = 0x5;
jberry	0:9b057566f9ee	153	if(Motor_command[i] == '\0') //keep popping chars until a delimiter is found
jberry	0:9b057566f9ee	154	break;
jberry	0:9b057566f9ee	155	else if(Motor_command[i] != '\0' && i == 99)
jberry	0:9b057566f9ee	156	Motor_command[i] = '\0';
jberry	0:9b057566f9ee	157
jberry	0:9b057566f9ee	158	}
jberry	0:9b057566f9ee	159
jberry	0:9b057566f9ee	160	//sprintf(MOT_Return_chars, "a command was received.\n");
jberry	0:9b057566f9ee	161	//TX_Channel.write(MOT_Return_chars, strlen(MOT_Return_chars)+1); //output the command for debugging
jberry	0:9b057566f9ee	162	//TX_Channel.write(Motor_command, strlen(Motor_command) +1);
jberry	0:9b057566f9ee	163
jberry	0:9b057566f9ee	164	//which motor?
jberry	0:9b057566f9ee	165	//if (strlen(Servo_Command) >= 20 && strstr(Servo_Command,":Calibrate"))
jberry	0:9b057566f9ee	166	if(strstr(Motor_command, "MOT:") != NULL)
jberry	0:9b057566f9ee	167	{
jberry	0:9b057566f9ee	168	Motor_number = strtod(1+strstr(Motor_command, ":"), &pEND);
jberry	0:9b057566f9ee	169	// motor_percentage = -1;
jberry	0:9b057566f9ee	170
jberry	0:9b057566f9ee	171	//sprintf(output_chars, "motor number is: %d\n", Motor_number);
jberry	0:9b057566f9ee	172	//TX_Channel.write(output_chars, strlen(output_chars)+1); //output the command for debugging
jberry	0:9b057566f9ee	173
jberry	0:9b057566f9ee	174
jberry	0:9b057566f9ee	175	if(Motor_number < 2) // valid conversion
jberry	0:9b057566f9ee	176	{
jberry	0:9b057566f9ee	177	if(Motor_number == 0)
jberry	0:9b057566f9ee	178	M = &Motor1;
jberry	0:9b057566f9ee	179	else
jberry	0:9b057566f9ee	180	M = &Motor2;
jberry	0:9b057566f9ee	181	motor_percentage = strtod(1+pEND, &pEND2);
jberry	0:9b057566f9ee	182	//sprintf(output_chars, "percentage is: %f\n", motor_percentage);
jberry	0:9b057566f9ee	183	//TX_Channel.write(output_chars, strlen(output_chars)+1); //output the command for debugging
jberry	0:9b057566f9ee	184	M->set_speed(motor_percentage*(-1));
jberry	0:9b057566f9ee	185	} //end valid conversion check
jberry	0:9b057566f9ee	186	} //end single MOT: command
jberry	0:9b057566f9ee	187
jberry	0:9b057566f9ee	188
jberry	0:9b057566f9ee	189	else if(strstr(Motor_command, "MO2:") != NULL)
jberry	0:9b057566f9ee	190	{
jberry	0:9b057566f9ee	191	motor_direction = strtod(&Motor_command[4], &pEND);
jberry	0:9b057566f9ee	192	if(motor_direction >= -1 && motor_direction <= 1)
jberry	0:9b057566f9ee	193	{
jberry	0:9b057566f9ee	194	motor_strength = strtod(1+pEND, &pEND2);
jberry	0:9b057566f9ee	195	if(motor_strength >= -1 && motor_strength <= 1)
jberry	0:9b057566f9ee	196	{
jberry	0:9b057566f9ee	197	//sprintf(output_chars, "Motor direction is: %f Motor strength is:%f\n", motor_direction, motor_strength);
jberry	0:9b057566f9ee	198	//TX_Channel.write(output_chars, strlen(output_chars)+1); //output the command for debugging
jberry	0:9b057566f9ee	199	if(motor_direction < 0)
jberry	0:9b057566f9ee	200	{
jberry	0:9b057566f9ee	201	Motor1.set_speed(motor_strength);
jberry	0:9b057566f9ee	202	Motor2.set_speed(motor_strengthabs(motor_direction)2);
jberry	0:9b057566f9ee	203	}
jberry	0:9b057566f9ee	204	else if(motor_direction>0)
jberry	0:9b057566f9ee	205	{
jberry	0:9b057566f9ee	206	Motor2.set_speed(motor_strength);
jberry	0:9b057566f9ee	207	Motor1.set_speed(motor_strengthabs(motor_direction)2);
jberry	0:9b057566f9ee	208
jberry	0:9b057566f9ee	209	}
jberry	0:9b057566f9ee	210	}
jberry	0:9b057566f9ee	211	}
jberry	0:9b057566f9ee	212
jberry	0:9b057566f9ee	213	}
jberry	0:9b057566f9ee	214	leds = 0x5;
jberry	0:9b057566f9ee	215	}
jberry	0:9b057566f9ee	216
jberry	0:9b057566f9ee	217
jberry	0:9b057566f9ee	218
jberry	0:9b057566f9ee	219	}
jberry	0:9b057566f9ee	220
jberry	0:9b057566f9ee	221
jberry	0:9b057566f9ee	222
jberry	0:9b057566f9ee	223	#endif
jberry	0:9b057566f9ee	224

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	01 Nov 2010
Imports:	6
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	1
Followers:	1

Hexacopter/Motor_control.h@0:9b057566f9ee, 2010-11-01 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning