Milestone1 | Mbed

Users » Ramonwaninge » Code » Milestone1

Ramon Waninge / Mbed 2 deprecated Milestone1

Dependencies: FastPWM mbed QEI biquadFilter HIDScope MODSERIAL

main.cpp@36:077fe5b3189b, 2018-10-31 (annotated)

Committer:: efvanmarrewijk
Date:: Wed Oct 31 10:24:01 2018 +0000
Revision:: 36:077fe5b3189b
Parent:: 35:ba556f2d0fcc
Child:: 38:681100ce5fb8

Working copy with only movement for motor 1, names changed to somethingl (for left motor)

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	35:ba556f2d0fcc	38	QEI Encoderr(D11,D10,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	33:ec07e11676ec	105	float Kp = 10.37f;
efvanmarrewijk	32:a5b411833d1e	106	float Ki = 1.02f;
efvanmarrewijk	33:ec07e11676ec	107	float Kd = 0.0514f;
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* turnl() // main function for movement of motor 1, all above functions with an extra tab are called
efvanmarrewijk	26:b48708ed51ff	134	{
efvanmarrewijk	35:ba556f2d0fcc	135	//float refvalue = pi/4.0f;
efvanmarrewijk	35:ba556f2d0fcc	136	float refvalue = DesiredAnglel(); // different minus sign per motor
efvanmarrewijk	35:ba556f2d0fcc	137	int counts = Countslinput(); // different encoder pins per motor
efvanmarrewijk	35:ba556f2d0fcc	138	float currentangle = CurrentAngle(counts); // different minus sign per motor
efvanmarrewijk	35:ba556f2d0fcc	139	float PIDcontrol = PIDcontroller(refvalue,currentangle); // same for every motor
efvanmarrewijk	35:ba556f2d0fcc	140	float error = ErrorCalc(refvalue,currentangle); // same for every motor
efvanmarrewijk	30:c4a3e868ef04	141
efvanmarrewijk	35:ba556f2d0fcc	142	pin6 = fabs(PIDcontrol); // different pins for every motor
efvanmarrewijk	35:ba556f2d0fcc	143	pin7 = PIDcontrol > 0.0f; // different pins for every motor
efvanmarrewijk	31:91ad5b188bd9	144	//pin6 = 0.4+0.6*fabs(PIDcontr); //geschaald
efvanmarrewijk	31:91ad5b188bd9	145	//pin6 = fabs(PIDcontr)/pi;
efvanmarrewijk	35:ba556f2d0fcc	146	//pc.printf(" error: %f ref: %f angle: %f \r\n",error,refvalue,currentangle);
efvanmarrewijk	33:ec07e11676ec	147	float* outcome;
efvanmarrewijk	35:ba556f2d0fcc	148	float turninfo[3] = {error, refvalue, currentangle};
efvanmarrewijk	35:ba556f2d0fcc	149	//float ( &fillarr( float (&outcome)[3] ) )[3] { // no decay; argument must be size 3
efvanmarrewijk	35:ba556f2d0fcc	150	outcome = turninfo;
efvanmarrewijk	35:ba556f2d0fcc	151
efvanmarrewijk	35:ba556f2d0fcc	152	return outcome;
efvanmarrewijk	35:ba556f2d0fcc	153	}
efvanmarrewijk	35:ba556f2d0fcc	154
efvanmarrewijk	32:a5b411833d1e	155	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	156	{ float MotorPosition = - (counts - offsetcounts) / fCountsRad;
efvanmarrewijk	25:76e9e5597416	157	// minus sign to correct for direction convention
efvanmarrewijk	32:a5b411833d1e	158	return MotorPosition;
efvanmarrewijk	25:76e9e5597416	159	}
efvanmarrewijk	11:3efd6a324f16	160
efvanmarrewijk	16:720365110953	161	// Main program
efvanmarrewijk	11:3efd6a324f16	162	int main()
efvanmarrewijk	18:ca084c362855	163	{
efvanmarrewijk	26:b48708ed51ff	164	pc.baud(115200);
efvanmarrewijk	32:a5b411833d1e	165	pin3.period_us(15); // If you give a period on one pin, c++ gives all pins this period
efvanmarrewijk	31:91ad5b188bd9	166
efvanmarrewijk	33:ec07e11676ec	167	//float motoroutcome1 = motor.attach(turn1,dt);
efvanmarrewijk	33:ec07e11676ec	168
efvanmarrewijk	26:b48708ed51ff	169	emergencybutton.rise(Emergency); //If the button is pressed, stop program
efvanmarrewijk	30:c4a3e868ef04	170	//pin6 = 0.01;
efvanmarrewijk	17:0ae9e8c958f8	171
efvanmarrewijk	16:720365110953	172	while (true)
efvanmarrewijk	25:76e9e5597416	173	{
efvanmarrewijk	35:ba556f2d0fcc	174	float* motoroutcomel = turnl();
efvanmarrewijk	35:ba556f2d0fcc	175	float motorl1 = motoroutcomel[0];
efvanmarrewijk	35:ba556f2d0fcc	176	float motorl2 = motoroutcomel[1];
efvanmarrewijk	35:ba556f2d0fcc	177	float motorl3 = motoroutcomel[2];
efvanmarrewijk	33:ec07e11676ec	178
efvanmarrewijk	35:ba556f2d0fcc	179	pc.printf(" error1: %f ref1: %f angle1: %f \r\n",motorl1,motorl2,motorl3);
efvanmarrewijk	33:ec07e11676ec	180	wait(dt);
efvanmarrewijk	33:ec07e11676ec	181	//wait(dt*10);
efvanmarrewijk	33:ec07e11676ec	182	//wait(printingfreq); --> still needs to be defined
Ramonwaninge	3:d39285fdd103	183	}
efvanmarrewijk	30:c4a3e868ef04	184	}