Floris Hoek / Mbed 2 deprecated template_biorobotics_Group_18

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:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@9:e8cc37a94fec, 2019-10-21 (annotated)

Committer:
Floris_Hoek
Date:
Mon Oct 21 12:17:39 2019 +0000
Revision:
9:e8cc37a94fec
Parent:
8:7dab565a208e
Child:
10:8c38a1a5b522
motorsoff, failuremode (not working)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
Floris_Hoek 8:7dab565a208e 2 #include "HIDScope.h"
Floris_Hoek 8:7dab565a208e 3 #include "BiQuad.h"
RobertoO 1:b862262a9d14 4 #include "MODSERIAL.h"
paulstuiver 2:75b2f713161c 5 #include "FastPWM.h"
paulstuiver 5:2ae500da8fe1 6 #include "QEI.h"
paulstuiver 5:2ae500da8fe1 7 #include <math.h>
paulstuiver 2:75b2f713161c 8
Floris_Hoek 8:7dab565a208e 9 #include "Motor_Control.h"
Floris_Hoek 8:7dab565a208e 10
paulstuiver 2:75b2f713161c 11 DigitalIn button1(D12);
Floris_Hoek 9:e8cc37a94fec 12 InterruptIn button2(SW2);
Floris_Hoek 9:e8cc37a94fec 13 DigitalOut ledr(LED_RED);
Floris_Hoek 9:e8cc37a94fec 14
Floris_Hoek 9:e8cc37a94fec 15 AnalogIn shield0(A0);
Floris_Hoek 9:e8cc37a94fec 16 AnalogIn shield1(A1);
Floris_Hoek 9:e8cc37a94fec 17 AnalogIn shield2(A2);
Floris_Hoek 9:e8cc37a94fec 18 AnalogIn shield3(A3);
Floris_Hoek 9:e8cc37a94fec 19
Floris_Hoek 8:7dab565a208e 20 Ticker measurecontrol;
paulstuiver 3:4f281c336a8b 21 DigitalOut motor1Direction(D7);
paulstuiver 3:4f281c336a8b 22 FastPWM motor1Velocity(D6);
Floris_Hoek 8:7dab565a208e 23 MODSERIAL pc(USBTX, USBRX);
paulstuiver 5:2ae500da8fe1 24 QEI Encoder(D8,D9,NC,8400);
Floris_Hoek 8:7dab565a208e 25
Floris_Hoek 9:e8cc37a94fec 26 float PI = 3.1415926;//535897932384626433832795028841971693993;
Floris_Hoek 9:e8cc37a94fec 27 float timeinterval = 0.001; // Time interval of the Ticker function
Floris_Hoek 9:e8cc37a94fec 28 bool whileloop = true; // Statement to keep the whileloop in the main function running
Floris_Hoek 9:e8cc37a94fec 29 // While loop has to stop running when failuremode is activated
Floris_Hoek 9:e8cc37a94fec 30
Floris_Hoek 9:e8cc37a94fec 31 // Define the different states in which the robot can be
Floris_Hoek 9:e8cc37a94fec 32 enum States {MOTORS_OFF, EMG_CALIBRATION, MOTOR_CALIBRATION,
Floris_Hoek 9:e8cc37a94fec 33 START_GAME, DEMO_MODE, GAME_MODE, MOVE_END_EFFECTOR,
Floris_Hoek 9:e8cc37a94fec 34 MOVE_GRIPPER, END_GAME, FAILURE_MODE};
Floris_Hoek 9:e8cc37a94fec 35
Floris_Hoek 9:e8cc37a94fec 36 // Default state is the state in which the motors are turned off
Floris_Hoek 9:e8cc37a94fec 37 States MyState = MOTORS_OFF;
paulstuiver 5:2ae500da8fe1 38
Floris_Hoek 9:e8cc37a94fec 39
Floris_Hoek 9:e8cc37a94fec 40
Floris_Hoek 9:e8cc37a94fec 41 void motorsoff() {
Floris_Hoek 9:e8cc37a94fec 42 bool aa = true; // Boolean for the while loop
Floris_Hoek 9:e8cc37a94fec 43 sendtomotor(0);
Floris_Hoek 9:e8cc37a94fec 44 while (aa) {
Floris_Hoek 9:e8cc37a94fec 45 if (button1.read() == 0) { // If button1 is pressed, set MyState to EMG_CALIBRATION and exit the while loop
Floris_Hoek 9:e8cc37a94fec 46 MyState = EMG_CALIBRATION;
Floris_Hoek 9:e8cc37a94fec 47 aa = false;
Floris_Hoek 9:e8cc37a94fec 48 }
Floris_Hoek 9:e8cc37a94fec 49 }
Floris_Hoek 9:e8cc37a94fec 50 }
paulstuiver 5:2ae500da8fe1 51
paulstuiver 5:2ae500da8fe1 52
paulstuiver 5:2ae500da8fe1 53 //P control implementation (behaves like a spring)
paulstuiver 5:2ae500da8fe1 54 double P_controller(double error)
paulstuiver 5:2ae500da8fe1 55 {
Floris_Hoek 8:7dab565a208e 56 double Kp = 17.5;
paulstuiver 5:2ae500da8fe1 57 //Proportional part:
paulstuiver 5:2ae500da8fe1 58 double u_k = Kp * error;
paulstuiver 5:2ae500da8fe1 59
paulstuiver 5:2ae500da8fe1 60 //sum all parts and return it
paulstuiver 5:2ae500da8fe1 61 return u_k;
paulstuiver 5:2ae500da8fe1 62 }
paulstuiver 2:75b2f713161c 63
Floris_Hoek 8:7dab565a208e 64 void nothing(){// Do nothing
paulstuiver 2:75b2f713161c 65 }
paulstuiver 2:75b2f713161c 66
Floris_Hoek 9:e8cc37a94fec 67
Floris_Hoek 9:e8cc37a94fec 68 void New_State() {
Floris_Hoek 9:e8cc37a94fec 69 switch (MyState)
Floris_Hoek 9:e8cc37a94fec 70 {
Floris_Hoek 9:e8cc37a94fec 71 case MOTORS_OFF :
Floris_Hoek 9:e8cc37a94fec 72 pc.printf("State: Motors turned off");
Floris_Hoek 9:e8cc37a94fec 73 motorsoff();
Floris_Hoek 9:e8cc37a94fec 74 break;
Floris_Hoek 9:e8cc37a94fec 75
Floris_Hoek 9:e8cc37a94fec 76 case EMG_CALIBRATION :
Floris_Hoek 9:e8cc37a94fec 77 pc.printf("State: EMG Calibration");
Floris_Hoek 9:e8cc37a94fec 78 break;
Floris_Hoek 9:e8cc37a94fec 79
Floris_Hoek 9:e8cc37a94fec 80 case MOTOR_CALIBRATION :
Floris_Hoek 9:e8cc37a94fec 81 pc.printf("State: Motor Calibration");
Floris_Hoek 9:e8cc37a94fec 82 break;
Floris_Hoek 9:e8cc37a94fec 83
Floris_Hoek 9:e8cc37a94fec 84 case START_GAME :
Floris_Hoek 9:e8cc37a94fec 85 pc.printf("State: Start game");
Floris_Hoek 9:e8cc37a94fec 86 break;
Floris_Hoek 9:e8cc37a94fec 87
Floris_Hoek 9:e8cc37a94fec 88 case DEMO_MODE :
Floris_Hoek 9:e8cc37a94fec 89 pc.printf("State: Demo mode");
Floris_Hoek 9:e8cc37a94fec 90 break;
Floris_Hoek 9:e8cc37a94fec 91
Floris_Hoek 9:e8cc37a94fec 92 case GAME_MODE :
Floris_Hoek 9:e8cc37a94fec 93 pc.printf("State: Game mode");
Floris_Hoek 9:e8cc37a94fec 94 break;
Floris_Hoek 9:e8cc37a94fec 95
Floris_Hoek 9:e8cc37a94fec 96 case MOVE_END_EFFECTOR :
Floris_Hoek 9:e8cc37a94fec 97 pc.printf("State: Move end effector");
Floris_Hoek 9:e8cc37a94fec 98 break;
Floris_Hoek 9:e8cc37a94fec 99
Floris_Hoek 9:e8cc37a94fec 100 case MOVE_GRIPPER :
Floris_Hoek 9:e8cc37a94fec 101 pc.printf("State: Move the gripper");
Floris_Hoek 9:e8cc37a94fec 102 break;
Floris_Hoek 9:e8cc37a94fec 103
Floris_Hoek 9:e8cc37a94fec 104 case END_GAME :
Floris_Hoek 9:e8cc37a94fec 105 pc.printf("State: End of the game");
Floris_Hoek 9:e8cc37a94fec 106 break;
Floris_Hoek 9:e8cc37a94fec 107
Floris_Hoek 9:e8cc37a94fec 108 case FAILURE_MODE :
Floris_Hoek 9:e8cc37a94fec 109 pc.printf("FAILURE MODE!!"); // Let the user know it is in failure mode
Floris_Hoek 9:e8cc37a94fec 110 ledr = 0; // Turn red led on to show that failure mode is active
Floris_Hoek 9:e8cc37a94fec 111 whileloop = false;
Floris_Hoek 9:e8cc37a94fec 112 break;
Floris_Hoek 9:e8cc37a94fec 113
Floris_Hoek 9:e8cc37a94fec 114 default :
Floris_Hoek 9:e8cc37a94fec 115 pc.printf("Default state: Motors are turned off");
Floris_Hoek 9:e8cc37a94fec 116 sendtomotor(0);
Floris_Hoek 9:e8cc37a94fec 117 break;
Floris_Hoek 9:e8cc37a94fec 118 }
Floris_Hoek 9:e8cc37a94fec 119 }
Floris_Hoek 9:e8cc37a94fec 120
Floris_Hoek 9:e8cc37a94fec 121 void failuremode() {
Floris_Hoek 9:e8cc37a94fec 122 MyState = FAILURE_MODE;
Floris_Hoek 9:e8cc37a94fec 123 New_State();
Floris_Hoek 9:e8cc37a94fec 124 }
Floris_Hoek 9:e8cc37a94fec 125
RobertoO 0:67c50348f842 126 int main()
RobertoO 0:67c50348f842 127 {
Floris_Hoek 9:e8cc37a94fec 128 pc.printf("Starting...\r\n\r\n");
Floris_Hoek 8:7dab565a208e 129 double frequency = 17000.0;
Floris_Hoek 8:7dab565a208e 130 double period_signal = 1.0/frequency;
RobertoO 0:67c50348f842 131 pc.baud(115200);
Floris_Hoek 9:e8cc37a94fec 132
Floris_Hoek 9:e8cc37a94fec 133 button2.fall(failuremode); // Function is always activated when the button is pressed
paulstuiver 2:75b2f713161c 134 motor1Velocity.period(period_signal);
Floris_Hoek 8:7dab565a208e 135 measurecontrol.attach(measureandcontrol, timeinterval);
Floris_Hoek 9:e8cc37a94fec 136
Floris_Hoek 9:e8cc37a94fec 137 int previous_state_int = (int)MyState;
Floris_Hoek 9:e8cc37a94fec 138 New_State();
Floris_Hoek 9:e8cc37a94fec 139
Floris_Hoek 9:e8cc37a94fec 140 while(whileloop)
paulstuiver 2:75b2f713161c 141 {
Floris_Hoek 9:e8cc37a94fec 142 if ( (previous_state_int - (int)MyState) != 0 ) {
Floris_Hoek 9:e8cc37a94fec 143 New_State();
Floris_Hoek 9:e8cc37a94fec 144 }
Floris_Hoek 9:e8cc37a94fec 145 previous_state_int = (int)MyState;
RobertoO 0:67c50348f842 146 }
Floris_Hoek 9:e8cc37a94fec 147
Floris_Hoek 8:7dab565a208e 148 pc.printf("Programm stops running\r\n");
Floris_Hoek 8:7dab565a208e 149 sendtomotor(0);
Floris_Hoek 8:7dab565a208e 150 measurecontrol.attach(nothing,10000);
Floris_Hoek 8:7dab565a208e 151 return 0;
Floris_Hoek 8:7dab565a208e 152 }