Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: HIDScope MODSERIAL QEI mbed
Fork of Burgerboy3000code by
main.cpp
- Committer:
- Frostworks
- Date:
- 2016-11-07
- Revision:
- 31:21a112643dc9
- Parent:
- 30:492595db0fc3
- Child:
- 32:952f3f30a0cd
File content as of revision 31:21a112643dc9:
#include "mbed.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "QEI.h"
DigitalOut led_g(LED_GREEN);
DigitalOut led_b(LED_BLUE);
DigitalOut led_r(LED_RED);
DigitalOut M1_Rotate(D2); // voltage only base rotation
PwmOut M1_Speed(D3); // voltage only base rotation
MODSERIAL pc(USBTX, USBRX);
//QEI wheel(PinName channelA, PinName channelB, PinName index, int pulsesPerRev, Encoding encoding=X2_ENCODING)
QEI motor2(D10,D11,NC,8400,QEI::X4_ENCODING);
QEI motor3(D12,D13,NC,8400,QEI::X4_ENCODING);
DigitalOut M2_Rotate(D4); // encoder side pot 2 translation
PwmOut M2_Speed(D5); // encoder side pot 2 translation
DigitalOut M3_Rotate(D7); // encoder side pot 1 spatel rotation
PwmOut M3_Speed(D6); // encoder side pot 1 spatel rotation
bool links;
bool rechts;
AnalogIn pot1(A4); // pot 1 motor 1
AnalogIn pot2(A3); // pot 2 motor 3
//Define objects
HIDScope scope( 2 );
AnalogIn emg0( A0 );
AnalogIn emg1( A1 );
DigitalIn buttonCalibrate(SW3);
DigitalIn buttonCalibrateComplete(SW2);
bool draairechts;
bool draailinks;
bool turn = 0;
float waiter = 0.12;
float translation = 0;
float degrees3 = 0;
float Puls_degree = (8400/360);
float wheel1 = 16;
float wheel2 = 31;
float wheel3 = 41;
float overbrenging = ((wheel2/wheel1)*(wheel3/wheel1));
float pi = 3.14159265359;
volatile float x;
volatile float x_prev =0;
volatile float b; // filtered 'output' of ReadAnalogInAndFilter
bool calibrate = false;
bool calibrate_complete = false;
double threshold_Left = 0;
double threshold_Right= 0;
Ticker sample_timer;
Ticker printinfo;
Ticker checkSetpointTranslation;
Ticker checkSetpointRotation;
DigitalOut led(LED1);
const double a1 = -1.6475;
const double a2 = 0.7009;
const double b0 = 0.8371;
const double b1 = -1.6742;
const double b2 = 0.8371;
const double c1 = -1.9645;
const double c2 = 0.9651;
const double d0 = 0.0001551;
const double d1 = 0.0003103;
const double d2 = 0.0001551;
double v1_HR = 0;
double v2_HR = 0;
double v1_LR = 0;
double v2_LR = 0;
double v1_HL = 0;
double v2_HL = 0;
double v1_LL = 0;
double v2_LL = 0;
double highpassFilterLeft = 0;
double lowpassFilterLeft = 0;
double highpassFilterRight = 0;
double lowpassFilterRight = 0;
//setpoints
volatile float setpointRotation;
volatile float setpointTranslation;
const double Setpoint_Translation = -200;
const double Setpoint_Back = 0;
const double Setpoint_Rotation = pi;
double M3_ControlSpeed = 0;
double M2_ControlSpeed = 0;
double SetpointError_Translation = 0;
double SetpointError_Rotation = 0;
double theta_translation;
double theta_rotation;
//gemiddelde
int counter = 0;
double Setpoint1 = 0;
double Setpoint2 = 0;
double Setpoint3 = 0;
double Setpoint4 = 0;
double Setpoint5 = 0;
double SetpointAvg = 0;
//booleans for control
bool booltranslate = false;
bool boolrotate = false;
//copied from slides
//Arm PID
const double Ts = 0.001953125; //Ts=1/fs (sample frequency)
const double Translation_Kp = 6.9, Translation_Ki = 0.8, Translation_Kd = 0.4;
double Translation_error = 0;
double Translation_e_prev = 0;
//Spatel PID
const double Rotation_Kp = 0.23, Rotation_Ki = 0.0429 , Rotation_Kd = 2;
//const double Rotation_Kp = 0.3, Rotation_Ki = 0.0429 , Rotation_Kd = 2; is best
//const double Rotation_Kp = 0.10, Rotation_Ki = 0.0429 , Rotation_Kd = 2;
//const double Rotation_Kp = 0.05, Rotation_Ki = 0.0429 , Rotation_Kd = 0.4;
double Rotation_error = 0;
double Rotation_e_prev = 0;
double pid_control(double error, const double kp, const double ki, const double kd, double &e_int, double &e_prev)
{
double e_der = (error - e_prev) / Ts;
e_prev = error;
e_int = e_int + (Ts * error);
return kp*error + ki + e_int + kd + e_der;
}
double biquad(double u, double&v1, double&v2, const double a1, const double a2, const double b0,
const double b1, const double b2)
{
double v = u - a1*v1 - a2*v2;
double y = b0*v + b1*v1 + b2*v2;
v2 = v1;
v1 = v;
return y;
}
/** Sample function
* this function samples the emg and sends it to HIDScope
**/
void filterSample()
{
highpassFilterLeft = fabs(biquad(emg0.read(), v1_HL, v2_HL, a1, a2, b0, b1, b2));
lowpassFilterLeft = biquad(highpassFilterLeft, v1_LL, v2_LL, c1, c2, d0, d1, d2);
//pc.printf("%f \n \r ", lowpassFilter);
highpassFilterRight = fabs(biquad(emg1.read(), v1_HR, v2_HR, a1, a2, b0, b1, b2));
lowpassFilterRight = biquad(highpassFilterRight, v1_LR, v2_LR, c1, c2, d0, d1, d2);
scope.set(0, lowpassFilterLeft );
scope.set(1, lowpassFilterRight );
scope.send();
//pc.printf("%f \n \r ", lowpassFilter);
}
/*void sample()
{
// Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope'
scope.set(0, emg0.read() );
scope.set(1, emg1.read() );
* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
* Ensure that enough channels are available (HIDScope scope( 2 ))
* Finally, send all channels to the PC at once
x = emg0; // Capture data scope.set(0, x); // store data in first element of scope memory
b = (x_prev + x)/2.0; // averaging filter
x_prev = x; // Prepare for next round
scope.send();
// To indicate that the function is working, the LED is toggled
led = !led;
pc.printf("%f, %f \n \r ", x, b);
} \*/
float GetPositionM2()
{
float pulses2 = motor2.getPulses();
float degrees2 = (pulses2/Puls_degree);
float radians2 = (degrees2/360)*2*pi;
float translation = ((radians2/overbrenging)*32.25);
return translation;
}
float GetRotationM3()
{
float pulses3 = motor3.getPulses();
float degrees3 = (pulses3/Puls_degree);
float radians3 = (degrees3/360)*2*pi;
return radians3;
}
void CheckErrorRotation()
{
counter++;
if (counter > 50) {
theta_rotation = GetRotationM3();
Setpoint5 = Setpoint4;
Setpoint4 = Setpoint3;
Setpoint3 = Setpoint2;
Setpoint2 = Setpoint1;
Setpoint1 = SetpointError_Rotation;
counter = 0;
}
SetpointError_Rotation = setpointRotation -theta_rotation;
SetpointAvg = ((SetpointError_Rotation + Setpoint1 + Setpoint2 + Setpoint3 + Setpoint4 + Setpoint5)/6);
}
void CheckErrorTranslation()
{
theta_translation = GetPositionM2();
SetpointError_Translation = setpointTranslation -theta_translation;
}
void motorRotation()
{
printf("setpoint = %f \n\r", setpointRotation);
//set direction
if (SetpointError_Rotation > 0) {
M3_Rotate = 0;
} else {
M3_Rotate = 1;
}
double speedfactor = 1;
if (setpointRotation != Setpoint_Rotation) {
speedfactor = 0.3;
}
double tolerance = 0.1;
if (setpointRotation == Setpoint_Rotation){
tolerance = 1;
}
M3_ControlSpeed = speedfactor * Ts * fabs( pid_control(SetpointError_Rotation, Rotation_Kp, Rotation_Ki, Rotation_Kd, Rotation_error, Rotation_e_prev));
if (fabs(SetpointAvg) < 0.1) {
M3_ControlSpeed = 0;
}
if (theta_rotation > tolerance)
boolrotate = true;
if ((fabs(theta_rotation) < tolerance ) && (M3_ControlSpeed == 0))
boolrotate = false;
M3_Speed = M3_ControlSpeed;
}
void motorTranslation()
{
theta_translation = GetPositionM2();
SetpointError_Translation = setpointTranslation - theta_translation;
//set direction
if (SetpointError_Translation < 0) {
M2_Rotate = 0;
} else {
M2_Rotate = 1;
}
M2_ControlSpeed = Ts * fabs( pid_control(SetpointError_Translation, Translation_Kp, Translation_Ki, Translation_Kd, Translation_error, Translation_e_prev));
if (fabs(SetpointError_Translation) < 8) {
M2_ControlSpeed = 0;
}
if ((theta_translation < -192) && (M2_ControlSpeed == 0))
booltranslate = true;
if ((theta_translation > -8) && (M2_ControlSpeed == 0))
booltranslate = false;
M2_Speed = M2_ControlSpeed;
}
void GoBack()
{
setpointTranslation = Setpoint_Back;
motorTranslation();
if (booltranslate == false) {
setpointRotation = Setpoint_Back;
motorRotation();
}
if (boolrotate == false) {
turn = 0;
}
led_r = 1;
led_b = 0;
}
void Burgerflip()
{
led_r = 0;
led_b = 1;
setpointTranslation = Setpoint_Translation;
motorTranslation();
if (booltranslate == true) {
setpointRotation = Setpoint_Rotation;
motorRotation();
}
}
void BurgerflipActie()
{
Burgerflip();
if (boolrotate == true) {
GoBack();
}
}
void print()
{
pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
}
void GetDirections()
{
if (lowpassFilterRight < threshold_Right)
rechts = 0;
if (lowpassFilterRight > threshold_Right)
rechts = 1;
if (lowpassFilterLeft < threshold_Left)
links = 0;
if (lowpassFilterLeft > threshold_Left)
links = 1;
pc.baud(115200);
if ((rechts == 1) && (links == 1) && (turn == 0)) {
draailinks = 0;
draairechts = 0;
turn = 1;
pc.printf("begin de actie \n \r ");
wait(waiter);
} else if ((rechts == 1) && (links == 1) && (turn == 1)) {
draailinks = 0;
draairechts = 0;
turn = 0;
pc.printf("breek de actie af \n \r ");
GoBack();
wait(waiter);
} else if ((rechts == 0) && (links == 0)&& (turn == 0)) {
} else if ((rechts == 1) && (draailinks == 0)&& (turn == 0)) {
/* if the right button is pressed and the motor isn't rotating to the left,
then start rotating to the right etc*/
draairechts = !draairechts;
pc.printf("draai naar rechts \n \r ");
wait(waiter);
} else if ((rechts == 1) && (draailinks == 1)&& (turn == 0)) {
draailinks = 0;
draairechts = !draairechts;
pc.printf("draai naar rechts na links \n \r ");
wait(waiter);
} else if ((links == 1) && (draairechts == 0)&& (turn == 0)) {
draailinks = !draailinks;
pc.printf("draai naar links \n \r ");
wait(waiter);
} else if ((links == 1) && (draairechts == 1) && (turn == 0)) {
draairechts = 0;
draailinks = !draailinks;
pc.printf("draai naar links na rechts \n \r ");
wait(waiter);
}
wait(2*waiter);
}
int main()
{
//Leds
led_g = 1;
led_b = 1;
led_r = 1;
/**Attach the 'sample' function to the timer 'sample_timer'.
* this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
*/
//sample_timer.attach(&sample, 0.001953125);
sample_timer.attach(&filterSample, Ts); //512 Hz
checkSetpointTranslation.attach(&CheckErrorTranslation,Ts);
checkSetpointRotation.attach(&CheckErrorRotation,Ts);
//printinfo.attach(&print, Ts);
pc.baud(115200);
pc.printf("please push the button to calibrate \n \r");
while (1) {
if (buttonCalibrate == 0) {
calibrate = true;
threshold_Left = lowpassFilterLeft*0.9;
threshold_Right = lowpassFilterRight*0.9;
pc.printf("calibration complete, press to continue \n \r");
}
if ((buttonCalibrateComplete == 0) && (calibrate == true)) {
calibrate_complete = true;
}
if (calibrate_complete == true) {
//pc.printf("calibration complete, calL = %f, L=%f CalR = %f, R = %f, boolL=%b boolR=%b \n \r", threshold_Left, lowpassFilterLeft, threshold_Right, lowpassFilterRight, links, rechts);
pc.printf("rotation is %f, setpoint %f, error = %f en translation = %f en de error %f \n \r", GetRotationM3(), Setpoint_Back, SetpointError_Rotation, GetPositionM2(), SetpointError_Translation);
GetDirections();
if (draairechts == true) {
M1_Speed = 0.1;
M1_Rotate = 1;
} else if (draailinks == true) {
M1_Speed = 0.1;
M1_Rotate = 0;
} else if (turn == 1) {
BurgerflipActie();
} else if (turn == 0) {
M2_Speed = 0;
M3_Speed = 0;
}
if ((draailinks == false) && (draairechts == false)) {
M1_Speed = 0;
}
}
}
}
