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@10:8c38a1a5b522, 2019-10-21 (annotated)

Committer:
Floris_Hoek
Date:
Mon Oct 21 12:44:20 2019 +0000
Revision:
10:8c38a1a5b522
Parent:
9:e8cc37a94fec
Child:
12:f4331640e3ad
States, Failure mode and Motors off state

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 10:8c38a1a5b522 26 float PI = 3.1415926f;//535897932384626433832795028841971693993;
Floris_Hoek 10:8c38a1a5b522 27 float timeinterval = 0.001f; // 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 10:8c38a1a5b522 43 sendtomotor(0.0f);
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 }
Floris_Hoek 10:8c38a1a5b522 51 /*
Floris_Hoek 10:8c38a1a5b522 52 void emgcalibration() {
Floris_Hoek 10:8c38a1a5b522 53 // do calibration
Floris_Hoek 10:8c38a1a5b522 54 }
Floris_Hoek 10:8c38a1a5b522 55 */
paulstuiver 5:2ae500da8fe1 56 //P control implementation (behaves like a spring)
paulstuiver 5:2ae500da8fe1 57 double P_controller(double error)
paulstuiver 5:2ae500da8fe1 58 {
Floris_Hoek 8:7dab565a208e 59 double Kp = 17.5;
paulstuiver 5:2ae500da8fe1 60 //Proportional part:
paulstuiver 5:2ae500da8fe1 61 double u_k = Kp * error;
paulstuiver 5:2ae500da8fe1 62
paulstuiver 5:2ae500da8fe1 63 //sum all parts and return it
paulstuiver 5:2ae500da8fe1 64 return u_k;
paulstuiver 5:2ae500da8fe1 65 }
paulstuiver 2:75b2f713161c 66
Floris_Hoek 8:7dab565a208e 67 void nothing(){// Do nothing
paulstuiver 2:75b2f713161c 68 }
paulstuiver 2:75b2f713161c 69
Floris_Hoek 9:e8cc37a94fec 70
Floris_Hoek 9:e8cc37a94fec 71 void New_State() {
Floris_Hoek 9:e8cc37a94fec 72 switch (MyState)
Floris_Hoek 9:e8cc37a94fec 73 {
Floris_Hoek 9:e8cc37a94fec 74 case MOTORS_OFF :
Floris_Hoek 9:e8cc37a94fec 75 pc.printf("State: Motors turned off");
Floris_Hoek 9:e8cc37a94fec 76 motorsoff();
Floris_Hoek 9:e8cc37a94fec 77 break;
Floris_Hoek 9:e8cc37a94fec 78
Floris_Hoek 9:e8cc37a94fec 79 case EMG_CALIBRATION :
Floris_Hoek 9:e8cc37a94fec 80 pc.printf("State: EMG Calibration");
Floris_Hoek 10:8c38a1a5b522 81 //measureandcontrol(emgcalibration,timeinterval);
Floris_Hoek 9:e8cc37a94fec 82 break;
Floris_Hoek 9:e8cc37a94fec 83
Floris_Hoek 9:e8cc37a94fec 84 case MOTOR_CALIBRATION :
Floris_Hoek 9:e8cc37a94fec 85 pc.printf("State: Motor Calibration");
Floris_Hoek 9:e8cc37a94fec 86 break;
Floris_Hoek 9:e8cc37a94fec 87
Floris_Hoek 9:e8cc37a94fec 88 case START_GAME :
Floris_Hoek 9:e8cc37a94fec 89 pc.printf("State: Start game");
Floris_Hoek 9:e8cc37a94fec 90 break;
Floris_Hoek 9:e8cc37a94fec 91
Floris_Hoek 9:e8cc37a94fec 92 case DEMO_MODE :
Floris_Hoek 9:e8cc37a94fec 93 pc.printf("State: Demo mode");
Floris_Hoek 9:e8cc37a94fec 94 break;
Floris_Hoek 9:e8cc37a94fec 95
Floris_Hoek 9:e8cc37a94fec 96 case GAME_MODE :
Floris_Hoek 9:e8cc37a94fec 97 pc.printf("State: Game mode");
Floris_Hoek 9:e8cc37a94fec 98 break;
Floris_Hoek 9:e8cc37a94fec 99
Floris_Hoek 9:e8cc37a94fec 100 case MOVE_END_EFFECTOR :
Floris_Hoek 9:e8cc37a94fec 101 pc.printf("State: Move end effector");
Floris_Hoek 9:e8cc37a94fec 102 break;
Floris_Hoek 9:e8cc37a94fec 103
Floris_Hoek 9:e8cc37a94fec 104 case MOVE_GRIPPER :
Floris_Hoek 9:e8cc37a94fec 105 pc.printf("State: Move the gripper");
Floris_Hoek 9:e8cc37a94fec 106 break;
Floris_Hoek 9:e8cc37a94fec 107
Floris_Hoek 9:e8cc37a94fec 108 case END_GAME :
Floris_Hoek 9:e8cc37a94fec 109 pc.printf("State: End of the game");
Floris_Hoek 9:e8cc37a94fec 110 break;
Floris_Hoek 9:e8cc37a94fec 111
Floris_Hoek 9:e8cc37a94fec 112 case FAILURE_MODE :
Floris_Hoek 9:e8cc37a94fec 113 pc.printf("FAILURE MODE!!"); // Let the user know it is in failure mode
Floris_Hoek 9:e8cc37a94fec 114 ledr = 0; // Turn red led on to show that failure mode is active
Floris_Hoek 9:e8cc37a94fec 115 whileloop = false;
Floris_Hoek 9:e8cc37a94fec 116 break;
Floris_Hoek 9:e8cc37a94fec 117
Floris_Hoek 9:e8cc37a94fec 118 default :
Floris_Hoek 9:e8cc37a94fec 119 pc.printf("Default state: Motors are turned off");
Floris_Hoek 10:8c38a1a5b522 120 sendtomotor(0.0f);
Floris_Hoek 9:e8cc37a94fec 121 break;
Floris_Hoek 9:e8cc37a94fec 122 }
Floris_Hoek 9:e8cc37a94fec 123 }
Floris_Hoek 9:e8cc37a94fec 124
Floris_Hoek 9:e8cc37a94fec 125 void failuremode() {
Floris_Hoek 9:e8cc37a94fec 126 MyState = FAILURE_MODE;
Floris_Hoek 9:e8cc37a94fec 127 New_State();
Floris_Hoek 9:e8cc37a94fec 128 }
Floris_Hoek 9:e8cc37a94fec 129
RobertoO 0:67c50348f842 130 int main()
RobertoO 0:67c50348f842 131 {
Floris_Hoek 9:e8cc37a94fec 132 pc.printf("Starting...\r\n\r\n");
Floris_Hoek 10:8c38a1a5b522 133 double frequency = 17000.0f;
Floris_Hoek 10:8c38a1a5b522 134 double period_signal = 1.0f/frequency;
RobertoO 0:67c50348f842 135 pc.baud(115200);
Floris_Hoek 9:e8cc37a94fec 136
Floris_Hoek 9:e8cc37a94fec 137 button2.fall(failuremode); // Function is always activated when the button is pressed
paulstuiver 2:75b2f713161c 138 motor1Velocity.period(period_signal);
Floris_Hoek 8:7dab565a208e 139 measurecontrol.attach(measureandcontrol, timeinterval);
Floris_Hoek 9:e8cc37a94fec 140
Floris_Hoek 9:e8cc37a94fec 141 int previous_state_int = (int)MyState;
Floris_Hoek 9:e8cc37a94fec 142 New_State();
Floris_Hoek 9:e8cc37a94fec 143
Floris_Hoek 9:e8cc37a94fec 144 while(whileloop)
paulstuiver 2:75b2f713161c 145 {
Floris_Hoek 9:e8cc37a94fec 146 if ( (previous_state_int - (int)MyState) != 0 ) {
Floris_Hoek 9:e8cc37a94fec 147 New_State();
Floris_Hoek 9:e8cc37a94fec 148 }
Floris_Hoek 9:e8cc37a94fec 149 previous_state_int = (int)MyState;
RobertoO 0:67c50348f842 150 }
Floris_Hoek 9:e8cc37a94fec 151
Floris_Hoek 8:7dab565a208e 152 pc.printf("Programm stops running\r\n");
Floris_Hoek 10:8c38a1a5b522 153 sendtomotor(0.0f);
Floris_Hoek 8:7dab565a208e 154 measurecontrol.attach(nothing,10000);
Floris_Hoek 8:7dab565a208e 155 return 0;
Floris_Hoek 8:7dab565a208e 156 }