Milestone1 | Mbed

Users » Ramonwaninge » Code » Milestone1

Ramon Waninge / Mbed 2 deprecated Milestone1

Dependencies: FastPWM mbed QEI biquadFilter HIDScope MODSERIAL

main.cpp@40:1be9dfad0a10, 2018-10-31 (annotated)

Committer:: efvanmarrewijk
Date:: Wed Oct 31 13:42:39 2018 +0000
Revision:: 40:1be9dfad0a10
Parent:: 39:dcf3e5019a63
Child:: 41:ca00cbb02f92
Child:: 42:cef1b3187e4c
Child:: 43:e8f2193822b7

Version for question about influencing motors

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	21:363271dcfe1f	12	InterruptIn button1(D0);
efvanmarrewijk	21:363271dcfe1f	13	InterruptIn button2(D1);
efvanmarrewijk	31:91ad5b188bd9	14	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	15
efvanmarrewijk	32:a5b411833d1e	16	DigitalIn pin8(D8); // Encoder 1 B
efvanmarrewijk	32:a5b411833d1e	17	DigitalIn pin9(D9); // Encoder 1 A
efvanmarrewijk	32:a5b411833d1e	18	DigitalIn pin10(D10); // Encoder 2 B
efvanmarrewijk	32:a5b411833d1e	19	DigitalIn pin11(D11); // Encoder 2 A
efvanmarrewijk	32:a5b411833d1e	20	DigitalIn pin12(D12); // Encoder 3 B
efvanmarrewijk	32:a5b411833d1e	21	DigitalIn pin13(D13); // Encoder 3 A
efvanmarrewijk	9:65c52c1f4a57	22
efvanmarrewijk	14:e21cb701ccb8	23	// Output
efvanmarrewijk	35:ba556f2d0fcc	24	DigitalOut pin2(D2); // Motor 3 direction = motor flip
efvanmarrewijk	35:ba556f2d0fcc	25	FastPWM pin3(D3); // Motor 3 pwm = motor flip
efvanmarrewijk	35:ba556f2d0fcc	26	DigitalOut pin4(D4); // Motor 2 direction = motor right
efvanmarrewijk	35:ba556f2d0fcc	27	FastPWM pin5(D5); // Motor 2 pwm = motor right
efvanmarrewijk	35:ba556f2d0fcc	28	FastPWM pin6(D6); // Motor 1 pwm = motor left
efvanmarrewijk	35:ba556f2d0fcc	29	DigitalOut pin7(D7); // Motor 1 direction = motor left
efvanmarrewijk	26:b48708ed51ff	30
efvanmarrewijk	26:b48708ed51ff	31	DigitalOut greenled(LED_GREEN,1);
efvanmarrewijk	26:b48708ed51ff	32	DigitalOut redled(LED_RED,1);
efvanmarrewijk	26:b48708ed51ff	33	DigitalOut blueled(LED_BLUE,1);
Ramonwaninge	2:d8a552d1d33a	34
efvanmarrewijk	16:720365110953	35	// Utilisation of libraries
efvanmarrewijk	16:720365110953	36	MODSERIAL pc(USBTX, USBRX);
efvanmarrewijk	35:ba556f2d0fcc	37	QEI Encoderl(D9,D8,NC,4200); // Counterclockwise motor rotation is the positive direction
efvanmarrewijk	39:dcf3e5019a63	38	QEI Encoderr(D10,D11,NC,4200); // Counterclockwise motor rotation is the positive direction
efvanmarrewijk	35:ba556f2d0fcc	39	QEI Encoderf(D13,D12,NC,4200); // Counterclockwise motor rotation is the positive direction
efvanmarrewijk	20:695140b8db2f	40	Ticker motor;
efvanmarrewijk	24:d255752065d1	41	Ticker encoders;
efvanmarrewijk	9:65c52c1f4a57	42
efvanmarrewijk	16:720365110953	43	// Global variables
efvanmarrewijk	32:a5b411833d1e	44	const float pi = 3.14159265358979f;
efvanmarrewijk	32:a5b411833d1e	45	float u3 = 0.0f; // Normalised variable for the movement of motor 3
efvanmarrewijk	32:a5b411833d1e	46	const float fCountsRad = 4200.0f;
efvanmarrewijk	32:a5b411833d1e	47	const float dt = 0.001f;
efvanmarrewijk	16:720365110953	48
efvanmarrewijk	16:720365110953	49	// Functions
efvanmarrewijk	32:a5b411833d1e	50	void Emergency() // Emgergency, if SW2 on biorobotics is pressed, everything will instantly abort and a red light goes on
efvanmarrewijk	32:a5b411833d1e	51	{ greenled = 1; // Red light on
efvanmarrewijk	30:c4a3e868ef04	52	blueled = 1;
efvanmarrewijk	30:c4a3e868ef04	53	redled = 0;
efvanmarrewijk	32:a5b411833d1e	54	pin3 = 0; // All motor forces to zero
efvanmarrewijk	30:c4a3e868ef04	55	pin5 = 0;
efvanmarrewijk	30:c4a3e868ef04	56	pin6 = 0;
efvanmarrewijk	32:a5b411833d1e	57	exit (0); // Abort mission!!
efvanmarrewijk	30:c4a3e868ef04	58	}
efvanmarrewijk	30:c4a3e868ef04	59
efvanmarrewijk	31:91ad5b188bd9	60	// Subfunctions
efvanmarrewijk	35:ba556f2d0fcc	61	int Countslinput() // Gets the counts from encoder 1
efvanmarrewijk	35:ba556f2d0fcc	62	{ int countsl;
efvanmarrewijk	35:ba556f2d0fcc	63	countsl = Encoderl.getPulses();
efvanmarrewijk	35:ba556f2d0fcc	64	return countsl;
efvanmarrewijk	27:3430dfb4c9fb	65	}
efvanmarrewijk	35:ba556f2d0fcc	66	int Countsrinput() // Gets the counts from encoder 2
efvanmarrewijk	35:ba556f2d0fcc	67	{ int countsr;
efvanmarrewijk	35:ba556f2d0fcc	68	countsr = Encoderr.getPulses();
efvanmarrewijk	35:ba556f2d0fcc	69	return countsr;
efvanmarrewijk	27:3430dfb4c9fb	70	}
efvanmarrewijk	35:ba556f2d0fcc	71	int Countsfinput() // Gets the counts from encoder 3
efvanmarrewijk	35:ba556f2d0fcc	72	{ int countsf;
efvanmarrewijk	35:ba556f2d0fcc	73	countsf = Encoderf.getPulses();
efvanmarrewijk	35:ba556f2d0fcc	74	return countsf;
efvanmarrewijk	27:3430dfb4c9fb	75	}
efvanmarrewijk	27:3430dfb4c9fb	76
efvanmarrewijk	32:a5b411833d1e	77	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	78	{ float angle = ((float)counts2.0fpi)/fCountsRad;
efvanmarrewijk	32:a5b411833d1e	79	while (angle > 2.0f*pi)
efvanmarrewijk	32:a5b411833d1e	80	{ angle = angle-2.0f*pi;
efvanmarrewijk	27:3430dfb4c9fb	81	}
efvanmarrewijk	32:a5b411833d1e	82	while (angle < -2.0f*pi)
efvanmarrewijk	32:a5b411833d1e	83	{ angle = angle+2.0f*pi;
efvanmarrewijk	30:c4a3e868ef04	84	}
efvanmarrewijk	30:c4a3e868ef04	85	return angle;
efvanmarrewijk	27:3430dfb4c9fb	86	}
efvanmarrewijk	27:3430dfb4c9fb	87
efvanmarrewijk	32:a5b411833d1e	88	float ErrorCalc(float refvalue,float CurAngle) // Calculates the error of the system, based on the current angle and the reference value
efvanmarrewijk	32:a5b411833d1e	89	{ float error = refvalue - CurAngle;
efvanmarrewijk	30:c4a3e868ef04	90	return error;
efvanmarrewijk	30:c4a3e868ef04	91	}
efvanmarrewijk	30:c4a3e868ef04	92
efvanmarrewijk	32:a5b411833d1e	93	float Kpcontr() // Sets the Kp value for the controller dependent on the scaled angle of potmeter 2
efvanmarrewijk	32:a5b411833d1e	94	{ float Kp = 20.0f*pot2;
efvanmarrewijk	30:c4a3e868ef04	95	return Kp;
efvanmarrewijk	30:c4a3e868ef04	96	}
efvanmarrewijk	30:c4a3e868ef04	97
efvanmarrewijk	32:a5b411833d1e	98	float Kdcontr() // Sets the Kd value for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk	32:a5b411833d1e	99	{ float Kd = 0.25f*pot1;
efvanmarrewijk	30:c4a3e868ef04	100	return Kd;
efvanmarrewijk	30:c4a3e868ef04	101	}
efvanmarrewijk	31:91ad5b188bd9	102
efvanmarrewijk	32:a5b411833d1e	103	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	104	{ //float Kp = Kpcontr();
efvanmarrewijk	39:dcf3e5019a63	105	float Kp = 10.42f;
efvanmarrewijk	32:a5b411833d1e	106	float Ki = 1.02f;
efvanmarrewijk	39:dcf3e5019a63	107	float Kd = 0.0493f;
efvanmarrewijk	30:c4a3e868ef04	108	//float Kd = Kdcontr();
efvanmarrewijk	32:a5b411833d1e	109	float error = ErrorCalc(refvalue,CurAngle);
efvanmarrewijk	30:c4a3e868ef04	110	static float error_integral = 0.0;
efvanmarrewijk	30:c4a3e868ef04	111	static float error_prev = error; // initialization with this value only done once!
efvanmarrewijk	31:91ad5b188bd9	112	static BiQuad PIDfilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
efvanmarrewijk	27:3430dfb4c9fb	113	// Proportional part:
efvanmarrewijk	30:c4a3e868ef04	114	float u_k = Kp * error;
efvanmarrewijk	27:3430dfb4c9fb	115	// Integral part
efvanmarrewijk	27:3430dfb4c9fb	116	error_integral = error_integral + error * dt;
efvanmarrewijk	30:c4a3e868ef04	117	float u_i = Ki * error_integral;
efvanmarrewijk	30:c4a3e868ef04	118	// Derivative part
efvanmarrewijk	30:c4a3e868ef04	119	float error_derivative = (error - error_prev)/dt;
efvanmarrewijk	31:91ad5b188bd9	120	float filtered_error_derivative = PIDfilter.step(error_derivative);
efvanmarrewijk	30:c4a3e868ef04	121	float u_d = Kd * filtered_error_derivative;
efvanmarrewijk	30:c4a3e868ef04	122	error_prev = error;
efvanmarrewijk	27:3430dfb4c9fb	123	// Sum all parts and return it
efvanmarrewijk	30:c4a3e868ef04	124	pc.printf ("Kp = %f Kd = %f",Kp,Kd);
efvanmarrewijk	30:c4a3e868ef04	125	return u_k + u_i + u_d;
efvanmarrewijk	27:3430dfb4c9fb	126	}
efvanmarrewijk	30:c4a3e868ef04	127
efvanmarrewijk	35:ba556f2d0fcc	128	float DesiredAnglel() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk	32:a5b411833d1e	129	{ float angle = (pot12.0fpi)-pi;
efvanmarrewijk	30:c4a3e868ef04	130	return angle;
efvanmarrewijk	30:c4a3e868ef04	131	}
efvanmarrewijk	35:ba556f2d0fcc	132
efvanmarrewijk	35:ba556f2d0fcc	133	float DesiredAngler() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk	35:ba556f2d0fcc	134	{ float angle = (pot22.0fpi)-pi;
efvanmarrewijk	35:ba556f2d0fcc	135	return angle;
efvanmarrewijk	35:ba556f2d0fcc	136	}
efvanmarrewijk	35:ba556f2d0fcc	137
efvanmarrewijk	35:ba556f2d0fcc	138	float* turnl() // main function for movement of motor 1, all above functions with an extra tab are called
efvanmarrewijk	26:b48708ed51ff	139	{
efvanmarrewijk	35:ba556f2d0fcc	140	//float refvalue = pi/4.0f;
efvanmarrewijk	35:ba556f2d0fcc	141	float refvalue = DesiredAnglel(); // different minus sign per motor
efvanmarrewijk	35:ba556f2d0fcc	142	int counts = Countslinput(); // different encoder pins per motor
efvanmarrewijk	35:ba556f2d0fcc	143	float currentangle = CurrentAngle(counts); // different minus sign per motor
efvanmarrewijk	35:ba556f2d0fcc	144	float PIDcontrol = PIDcontroller(refvalue,currentangle); // same for every motor
efvanmarrewijk	35:ba556f2d0fcc	145	float error = ErrorCalc(refvalue,currentangle); // same for every motor
efvanmarrewijk	30:c4a3e868ef04	146
efvanmarrewijk	35:ba556f2d0fcc	147	pin6 = fabs(PIDcontrol); // different pins for every motor
efvanmarrewijk	35:ba556f2d0fcc	148	pin7 = PIDcontrol > 0.0f; // different pins for every motor
efvanmarrewijk	31:91ad5b188bd9	149	//pin6 = 0.4+0.6*fabs(PIDcontr); //geschaald
efvanmarrewijk	31:91ad5b188bd9	150	//pin6 = fabs(PIDcontr)/pi;
efvanmarrewijk	35:ba556f2d0fcc	151	//pc.printf(" error: %f ref: %f angle: %f \r\n",error,refvalue,currentangle);
efvanmarrewijk	33:ec07e11676ec	152	float* outcome;
efvanmarrewijk	35:ba556f2d0fcc	153	float turninfo[3] = {error, refvalue, currentangle};
efvanmarrewijk	35:ba556f2d0fcc	154	//float ( &fillarr( float (&outcome)[3] ) )[3] { // no decay; argument must be size 3
efvanmarrewijk	35:ba556f2d0fcc	155	outcome = turninfo;
efvanmarrewijk	35:ba556f2d0fcc	156	return outcome;
efvanmarrewijk	35:ba556f2d0fcc	157	}
efvanmarrewijk	35:ba556f2d0fcc	158
efvanmarrewijk	35:ba556f2d0fcc	159	float* turnr() // main function for movement of motor 1, all above functions with an extra tab are called
efvanmarrewijk	35:ba556f2d0fcc	160	{
efvanmarrewijk	35:ba556f2d0fcc	161	//float refvalue = pi/4.0f;
efvanmarrewijk	35:ba556f2d0fcc	162	float refvalue = DesiredAngler(); // different minus sign per motor
efvanmarrewijk	35:ba556f2d0fcc	163	int counts = Countsrinput(); // different encoder pins per motor
efvanmarrewijk	35:ba556f2d0fcc	164	float currentangle = CurrentAngle(counts); // different minus sign per motor
efvanmarrewijk	35:ba556f2d0fcc	165	float PIDcontrol = PIDcontroller(refvalue,currentangle); // same for every motor
efvanmarrewijk	35:ba556f2d0fcc	166	float error = ErrorCalc(refvalue,currentangle); // same for every motor
efvanmarrewijk	35:ba556f2d0fcc	167
efvanmarrewijk	37:c61d7768c18a	168	pin5 = fabs(PIDcontrol); // different pins for every motor
efvanmarrewijk	37:c61d7768c18a	169	pin4 = PIDcontrol > 0.0f; // different pins for every motor
efvanmarrewijk	35:ba556f2d0fcc	170	//pin6 = 0.4+0.6*fabs(PIDcontr); //geschaald
efvanmarrewijk	35:ba556f2d0fcc	171	//pin6 = fabs(PIDcontr)/pi;
efvanmarrewijk	35:ba556f2d0fcc	172	//pc.printf(" error: %f ref: %f angle: %f \r\n",error,refvalue,currentangle);
efvanmarrewijk	35:ba556f2d0fcc	173	float* outcome;
efvanmarrewijk	35:ba556f2d0fcc	174	float turninfo[3] = {error, refvalue, currentangle};
efvanmarrewijk	33:ec07e11676ec	175	//float ( &fillarr( float (&outcome)[3] ) )[3] { // no decay; argument must be size 3
efvanmarrewijk	33:ec07e11676ec	176	outcome = turninfo;
efvanmarrewijk	33:ec07e11676ec	177	return outcome;
efvanmarrewijk	18:ca084c362855	178	}
efvanmarrewijk	25:76e9e5597416	179
efvanmarrewijk	32:a5b411833d1e	180	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	181	{ float MotorPosition = - (counts - offsetcounts) / fCountsRad;
efvanmarrewijk	25:76e9e5597416	182	// minus sign to correct for direction convention
efvanmarrewijk	32:a5b411833d1e	183	return MotorPosition;
efvanmarrewijk	25:76e9e5597416	184	}
efvanmarrewijk	11:3efd6a324f16	185
efvanmarrewijk	16:720365110953	186	// Main program
efvanmarrewijk	11:3efd6a324f16	187	int main()
efvanmarrewijk	18:ca084c362855	188	{
efvanmarrewijk	26:b48708ed51ff	189	pc.baud(115200);
efvanmarrewijk	32:a5b411833d1e	190	pin3.period_us(15); // If you give a period on one pin, c++ gives all pins this period
efvanmarrewijk	31:91ad5b188bd9	191
efvanmarrewijk	33:ec07e11676ec	192	//float motoroutcome1 = motor.attach(turn1,dt);
efvanmarrewijk	33:ec07e11676ec	193
efvanmarrewijk	26:b48708ed51ff	194	emergencybutton.rise(Emergency); //If the button is pressed, stop program
efvanmarrewijk	37:c61d7768c18a	195
efvanmarrewijk	16:720365110953	196	while (true)
efvanmarrewijk	25:76e9e5597416	197	{
efvanmarrewijk	40:1be9dfad0a10	198	turnl();
efvanmarrewijk	40:1be9dfad0a10	199	turnr();
efvanmarrewijk	40:1be9dfad0a10	200	wait(dt);
efvanmarrewijk	40:1be9dfad0a10	201	//float* motoroutcomel = turnl();
efvanmarrewijk	40:1be9dfad0a10	202	//float motorl1 = motoroutcomel[0];
efvanmarrewijk	40:1be9dfad0a10	203	//float motorl2 = motoroutcomel[1];
efvanmarrewijk	40:1be9dfad0a10	204	//float motorl3 = motoroutcomel[2];
efvanmarrewijk	40:1be9dfad0a10	205	//pc.printf(" errorl: %f refl: %f anglel: %f \r\n",motorl1,motorl2,motorl3);
efvanmarrewijk	33:ec07e11676ec	206
efvanmarrewijk	40:1be9dfad0a10	207	//float* motoroutcomer = turnr();
efvanmarrewijk	40:1be9dfad0a10	208	//float motorr1 = motoroutcomer[0];
efvanmarrewijk	40:1be9dfad0a10	209	//float motorr2 = motoroutcomer[1];
efvanmarrewijk	40:1be9dfad0a10	210	//float motorr3 = motoroutcomer[2];
efvanmarrewijk	40:1be9dfad0a10	211	//pc.printf(" errorr: %f refr: %f angler: %f \r\n",motorr1,motorr2,motorr3);
efvanmarrewijk	40:1be9dfad0a10	212
efvanmarrewijk	40:1be9dfad0a10	213	//wait(dt);
efvanmarrewijk	33:ec07e11676ec	214	//wait(dt*10);
efvanmarrewijk	33:ec07e11676ec	215	//wait(printingfreq); --> still needs to be defined
Ramonwaninge	3:d39285fdd103	216	}
efvanmarrewijk	30:c4a3e868ef04	217	}