Milestone1 | Mbed

Users » Ramonwaninge » Code » Milestone1

Ramon Waninge / Mbed 2 deprecated Milestone1

Dependencies: FastPWM mbed QEI biquadFilter HIDScope MODSERIAL

main.cpp@34:30100c1901d4, 2018-10-31 (annotated)

Committer:: efvanmarrewijk
Date:: Wed Oct 31 09:56:35 2018 +0000
Revision:: 34:30100c1901d4
Parent:: 32:a5b411833d1e

Motor movement in ticker, names of variables for motors changed to 1st motor = l (left), 2nd motor = r (right), 3nd motor = f (flip)

Who changed what in which revision?

User	Revision	Line number	New contents of line
efvanmarrewijk	16:720365110953	1	// Inclusion of libraries
efvanmarrewijk	31:91ad5b188bd9	2	#include "mbed.h"
efvanmarrewijk	31:91ad5b188bd9	3	#include "FastPWM.h"
efvanmarrewijk	31:91ad5b188bd9	4	#include "QEI.h" // Includes library for encoder
efvanmarrewijk	31:91ad5b188bd9	5	#include "MODSERIAL.h"
efvanmarrewijk	31:91ad5b188bd9	6	#include "HIDScope.h"
efvanmarrewijk	31:91ad5b188bd9	7	#include "BiQuad.h"
Ramonwaninge	0:3ea1bbfbeaae	8
efvanmarrewijk	14:e21cb701ccb8	9	// Input
efvanmarrewijk	27:3430dfb4c9fb	10	AnalogIn pot1(A1);
efvanmarrewijk	27:3430dfb4c9fb	11	AnalogIn pot2(A2);
efvanmarrewijk	34:30100c1901d4	12	InterruptIn buttonbio1(D0);
efvanmarrewijk	34:30100c1901d4	13	InterruptIn buttonbio2(D1);
efvanmarrewijk	34:30100c1901d4	14	InterruptIn buttonK64F(SW3);
efvanmarrewijk	31:91ad5b188bd9	15	InterruptIn emergencybutton(SW2); //The button SW2 on the K64F is the emergency button: if you press this, everything will abort as soon as possible
efvanmarrewijk	21:363271dcfe1f	16
efvanmarrewijk	32:a5b411833d1e	17	DigitalIn pin8(D8); // Encoder 1 B
efvanmarrewijk	32:a5b411833d1e	18	DigitalIn pin9(D9); // Encoder 1 A
efvanmarrewijk	32:a5b411833d1e	19	DigitalIn pin10(D10); // Encoder 2 B
efvanmarrewijk	32:a5b411833d1e	20	DigitalIn pin11(D11); // Encoder 2 A
efvanmarrewijk	32:a5b411833d1e	21	DigitalIn pin12(D12); // Encoder 3 B
efvanmarrewijk	32:a5b411833d1e	22	DigitalIn pin13(D13); // Encoder 3 A
efvanmarrewijk	9:65c52c1f4a57	23
efvanmarrewijk	14:e21cb701ccb8	24	// Output
efvanmarrewijk	34:30100c1901d4	25	DigitalOut pin2(D2); // Motor 3 direction = motor flip
efvanmarrewijk	34:30100c1901d4	26	FastPWM pin3(D3); // Motor 3 pwm = motor flip
efvanmarrewijk	34:30100c1901d4	27	DigitalOut pin4(D4); // Motor 2 direction = motor right
efvanmarrewijk	34:30100c1901d4	28	FastPWM pin5(D5); // Motor 2 pwm = motor right
efvanmarrewijk	34:30100c1901d4	29	FastPWM pin6(D6); // Motor 1 pwm = motor left
efvanmarrewijk	34:30100c1901d4	30	DigitalOut pin7(D7); // Motor 1 direction = motor left
efvanmarrewijk	26:b48708ed51ff	31
efvanmarrewijk	26:b48708ed51ff	32	DigitalOut greenled(LED_GREEN,1);
efvanmarrewijk	26:b48708ed51ff	33	DigitalOut redled(LED_RED,1);
efvanmarrewijk	26:b48708ed51ff	34	DigitalOut blueled(LED_BLUE,1);
Ramonwaninge	2:d8a552d1d33a	35
efvanmarrewijk	16:720365110953	36	// Utilisation of libraries
efvanmarrewijk	16:720365110953	37	MODSERIAL pc(USBTX, USBRX);
efvanmarrewijk	34:30100c1901d4	38	QEI Encoderl(D9,D8,NC,4200); // Counterclockwise motor rotation is the positive direction
efvanmarrewijk	34:30100c1901d4	39	QEI Encoderr(D11,D10,NC,4200); // Counterclockwise motor rotation is the positive direction
efvanmarrewijk	34:30100c1901d4	40	QEI Encoderf(D13,D12,NC,4200); // Counterclockwise motor rotation is the positive direction
efvanmarrewijk	20:695140b8db2f	41	Ticker motor;
efvanmarrewijk	24:d255752065d1	42	Ticker encoders;
efvanmarrewijk	9:65c52c1f4a57	43
efvanmarrewijk	16:720365110953	44	// Global variables
efvanmarrewijk	32:a5b411833d1e	45	const float pi = 3.14159265358979f;
efvanmarrewijk	32:a5b411833d1e	46	float u3 = 0.0f; // Normalised variable for the movement of motor 3
efvanmarrewijk	32:a5b411833d1e	47	const float fCountsRad = 4200.0f;
efvanmarrewijk	32:a5b411833d1e	48	const float dt = 0.001f;
efvanmarrewijk	16:720365110953	49
efvanmarrewijk	16:720365110953	50	// Functions
efvanmarrewijk	32:a5b411833d1e	51	void Emergency() // Emgergency, if SW2 on biorobotics is pressed, everything will instantly abort and a red light goes on
efvanmarrewijk	32:a5b411833d1e	52	{ greenled = 1; // Red light on
efvanmarrewijk	30:c4a3e868ef04	53	blueled = 1;
efvanmarrewijk	30:c4a3e868ef04	54	redled = 0;
efvanmarrewijk	32:a5b411833d1e	55	pin3 = 0; // All motor forces to zero
efvanmarrewijk	30:c4a3e868ef04	56	pin5 = 0;
efvanmarrewijk	30:c4a3e868ef04	57	pin6 = 0;
efvanmarrewijk	32:a5b411833d1e	58	exit (0); // Abort mission!!
efvanmarrewijk	30:c4a3e868ef04	59	}
efvanmarrewijk	30:c4a3e868ef04	60
efvanmarrewijk	31:91ad5b188bd9	61	// Subfunctions
efvanmarrewijk	34:30100c1901d4	62	int Countslinput() // Gets the counts from encoder 1
efvanmarrewijk	34:30100c1901d4	63	{ int countsl;
efvanmarrewijk	34:30100c1901d4	64	countsl = Encoderl.getPulses();
efvanmarrewijk	34:30100c1901d4	65	return countsl;
efvanmarrewijk	27:3430dfb4c9fb	66	}
efvanmarrewijk	34:30100c1901d4	67	int Countsrinput() // Gets the counts from encoder 2
efvanmarrewijk	34:30100c1901d4	68	{ int countsr;
efvanmarrewijk	34:30100c1901d4	69	countsr = Encoderr.getPulses();
efvanmarrewijk	34:30100c1901d4	70	return countsr;
efvanmarrewijk	27:3430dfb4c9fb	71	}
efvanmarrewijk	34:30100c1901d4	72	int Countsfinput() // Gets the counts from encoder3
efvanmarrewijk	34:30100c1901d4	73	{ int countsf;
efvanmarrewijk	34:30100c1901d4	74	countsf = Encoderf.getPulses();
efvanmarrewijk	34:30100c1901d4	75	return countsf;
efvanmarrewijk	27:3430dfb4c9fb	76	}
efvanmarrewijk	27:3430dfb4c9fb	77
efvanmarrewijk	32:a5b411833d1e	78	float CurrentAngle(float counts) // Calculates the current angle of the motor (between -2pi to 2pi) based on the counts from the encoder
efvanmarrewijk	32:a5b411833d1e	79	{ float angle = ((float)counts2.0fpi)/fCountsRad;
efvanmarrewijk	32:a5b411833d1e	80	while (angle > 2.0f*pi)
efvanmarrewijk	32:a5b411833d1e	81	{ angle = angle-2.0f*pi;
efvanmarrewijk	27:3430dfb4c9fb	82	}
efvanmarrewijk	32:a5b411833d1e	83	while (angle < -2.0f*pi)
efvanmarrewijk	32:a5b411833d1e	84	{ angle = angle+2.0f*pi;
efvanmarrewijk	30:c4a3e868ef04	85	}
efvanmarrewijk	30:c4a3e868ef04	86	return angle;
efvanmarrewijk	27:3430dfb4c9fb	87	}
efvanmarrewijk	27:3430dfb4c9fb	88
efvanmarrewijk	32:a5b411833d1e	89	float ErrorCalc(float refvalue,float CurAngle) // Calculates the error of the system, based on the current angle and the reference value
efvanmarrewijk	32:a5b411833d1e	90	{ float error = refvalue - CurAngle;
efvanmarrewijk	30:c4a3e868ef04	91	return error;
efvanmarrewijk	30:c4a3e868ef04	92	}
efvanmarrewijk	30:c4a3e868ef04	93
efvanmarrewijk	32:a5b411833d1e	94	float Kpcontr() // Sets the Kp value for the controller dependent on the scaled angle of potmeter 2
efvanmarrewijk	32:a5b411833d1e	95	{ float Kp = 20.0f*pot2;
efvanmarrewijk	30:c4a3e868ef04	96	return Kp;
efvanmarrewijk	30:c4a3e868ef04	97	}
efvanmarrewijk	30:c4a3e868ef04	98
efvanmarrewijk	32:a5b411833d1e	99	float Kdcontr() // Sets the Kd value for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk	32:a5b411833d1e	100	{ float Kd = 0.25f*pot1;
efvanmarrewijk	30:c4a3e868ef04	101	return Kd;
efvanmarrewijk	30:c4a3e868ef04	102	}
efvanmarrewijk	31:91ad5b188bd9	103
efvanmarrewijk	32:a5b411833d1e	104	float PIDcontroller(float refvalue,float CurAngle) // PID controller for the motors, based on the reference value and the current angle of the motor
efvanmarrewijk	30:c4a3e868ef04	105	{ //float Kp = Kpcontr();
efvanmarrewijk	32:a5b411833d1e	106	float Kp = 10.42f;
efvanmarrewijk	32:a5b411833d1e	107	float Ki = 1.02f;
efvanmarrewijk	32:a5b411833d1e	108	float Kd = 0.0493f;
efvanmarrewijk	30:c4a3e868ef04	109	//float Kd = Kdcontr();
efvanmarrewijk	32:a5b411833d1e	110	float error = ErrorCalc(refvalue,CurAngle);
efvanmarrewijk	30:c4a3e868ef04	111	static float error_integral = 0.0;
efvanmarrewijk	30:c4a3e868ef04	112	static float error_prev = error; // initialization with this value only done once!
efvanmarrewijk	31:91ad5b188bd9	113	static BiQuad PIDfilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
efvanmarrewijk	27:3430dfb4c9fb	114	// Proportional part:
efvanmarrewijk	30:c4a3e868ef04	115	float u_k = Kp * error;
efvanmarrewijk	27:3430dfb4c9fb	116	// Integral part
efvanmarrewijk	27:3430dfb4c9fb	117	error_integral = error_integral + error * dt;
efvanmarrewijk	30:c4a3e868ef04	118	float u_i = Ki * error_integral;
efvanmarrewijk	30:c4a3e868ef04	119	// Derivative part
efvanmarrewijk	30:c4a3e868ef04	120	float error_derivative = (error - error_prev)/dt;
efvanmarrewijk	31:91ad5b188bd9	121	float filtered_error_derivative = PIDfilter.step(error_derivative);
efvanmarrewijk	30:c4a3e868ef04	122	float u_d = Kd * filtered_error_derivative;
efvanmarrewijk	30:c4a3e868ef04	123	error_prev = error;
efvanmarrewijk	27:3430dfb4c9fb	124	// Sum all parts and return it
efvanmarrewijk	34:30100c1901d4	125	//pc.printf ("Kp = %f Kd = %f",Kp,Kd);
efvanmarrewijk	30:c4a3e868ef04	126	return u_k + u_i + u_d;
efvanmarrewijk	27:3430dfb4c9fb	127	}
efvanmarrewijk	30:c4a3e868ef04	128
efvanmarrewijk	32:a5b411833d1e	129	float DesiredAngle() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk	32:a5b411833d1e	130	{ float angle = (pot12.0fpi)-pi;
efvanmarrewijk	30:c4a3e868ef04	131	return angle;
efvanmarrewijk	30:c4a3e868ef04	132	}
efvanmarrewijk	27:3430dfb4c9fb	133
efvanmarrewijk	34:30100c1901d4	134	void turnl() // main function for movement of motor 1, all above functions with an extra tab are called
efvanmarrewijk	26:b48708ed51ff	135	{
efvanmarrewijk	31:91ad5b188bd9	136	//float refvalue1 = pi/4.0;
efvanmarrewijk	34:30100c1901d4	137	float refvalue = DesiredAngle();
efvanmarrewijk	34:30100c1901d4	138	int counts = Counts1input();
efvanmarrewijk	34:30100c1901d4	139	float currentangle = CurrentAngle(counts);
efvanmarrewijk	34:30100c1901d4	140	float PIDcontrol = PIDcontroller(refvalue,currentangle);
efvanmarrewijk	34:30100c1901d4	141	float error = ErrorCalc(refvalue,currentangle);
efvanmarrewijk	30:c4a3e868ef04	142
efvanmarrewijk	34:30100c1901d4	143	pin6 = fabs(PIDcontrol);
efvanmarrewijk	34:30100c1901d4	144	pin7 = PIDcontrol > 0.0f;
efvanmarrewijk	31:91ad5b188bd9	145	//pin6 = 0.4+0.6*fabs(PIDcontr); //geschaald
efvanmarrewijk	31:91ad5b188bd9	146	//pin6 = fabs(PIDcontr)/pi;
efvanmarrewijk	34:30100c1901d4	147	//pc.printf(" error: %f ref: %f angle: %f \r\n",error,refvalue,currentangle);
efvanmarrewijk	18:ca084c362855	148	}
efvanmarrewijk	25:76e9e5597416	149
efvanmarrewijk	32:a5b411833d1e	150	float ActualPosition(int counts, int offsetcounts) // After calibration, this function is used to return the counts (and thus the angle of the system) to 0
efvanmarrewijk	32:a5b411833d1e	151	{ float MotorPosition = - (counts - offsetcounts) / fCountsRad;
efvanmarrewijk	25:76e9e5597416	152	// minus sign to correct for direction convention
efvanmarrewijk	32:a5b411833d1e	153	return MotorPosition;
efvanmarrewijk	25:76e9e5597416	154	}
efvanmarrewijk	11:3efd6a324f16	155
efvanmarrewijk	16:720365110953	156	// Main program
efvanmarrewijk	11:3efd6a324f16	157	int main()
efvanmarrewijk	18:ca084c362855	158	{
efvanmarrewijk	26:b48708ed51ff	159	pc.baud(115200);
efvanmarrewijk	32:a5b411833d1e	160	pin3.period_us(15); // If you give a period on one pin, c++ gives all pins this period
efvanmarrewijk	31:91ad5b188bd9	161
efvanmarrewijk	34:30100c1901d4	162	motor.attach(turnl,dt);
efvanmarrewijk	28:aec0d9bdb949	163
efvanmarrewijk	26:b48708ed51ff	164	emergencybutton.rise(Emergency); //If the button is pressed, stop program
efvanmarrewijk	30:c4a3e868ef04	165	//pin6 = 0.01;
efvanmarrewijk	17:0ae9e8c958f8	166
efvanmarrewijk	16:720365110953	167	while (true)
efvanmarrewijk	25:76e9e5597416	168	{
efvanmarrewijk	34:30100c1901d4	169	//turnl();
efvanmarrewijk	34:30100c1901d4	170	//wait(dt);
Ramonwaninge	3:d39285fdd103	171	}
efvanmarrewijk	30:c4a3e868ef04	172	}