Group 9 BioRobotics / Mbed 2 deprecated the_code

Dependencies:   HIDScope Servo mbed QEI biquadFilter

main.cpp

Committer:
s1725696
Date:
2018-10-22
Revision:
5:3581013d4505
Parent:
4:8183e7b228f0
Child:
6:f495a77c2c95

File content as of revision 5:3581013d4505:

#include "mbed.h" // Use revision 119!!
#include "HIDScope.h" // For displaying data, select MBED - HID device, restart for every new code 
#include "QEI.h" // For reading the encoder of the motors 

#define SERIAL_BAUD 115200

// In- en outputs 
// -----------------------------------------------------------------------------

/*
// EMG related
AnalogIn emg1(); // EMG signal 1
AnalogIn emg2(); // EMG signal 2
// if needed
AnalogIn emg3(); // EMG signal 3
AnalogIn emg4(); // EMG signal 4
*/

// Motor related
DigitalOut dirpin_1(D4); // direction of motor 1
PwmOut pwmpin_1(D5); // PWM pin of motor 1
DigitalOut dirpin_2(D7); // direction of motor 2
PwmOut pwmpin_2(D6); // PWM pin of motor 2

// Extra stuff
// Like LED lights, buttons etc 
/*
DigitalIn button_motorcal(); // button for motor calibration
DigitalIn button_emergency(); // button for emergency mode
DigitalIn button_start(); // button for start mode (from demo mode)
DigitalIn button_demo(); // button for demo mode 
*/
DigitalIn led_red(LED_RED); // red led 
DigitalIn led_green(LED_GREEN); // green led
DigitalIn led_blue(LED_BLUE); // blue led
AnalogIn pot_1(A1); // potmeter for simulating EMG input


// Other stuff
// -----------------------------------------------------------------------------
// Define stuff like tickers etc

// Ticker NAME; // Name a ticker, use each ticker only for 1 function! 
HIDScope scope(2); // Number of channels in HIDScope
QEI Encoder(D13,D12,NC,64,QEI::X4_ENCODING); // Define the type of encoding: X4 encoding(default is X2)
Serial pc(USBTX,USBRX);

// Variables 
// ----------------------------------------------------------------------------- 
// Define here all variables needed throughout the whole code 
int counts;

// Functions
// -----------------------------------------------------------------------------
 
// Encoder 
// Getting encoder information from motors
int encoder()
{
    int counts = Encoder.getPulses();
    return counts;
    } 
    
// potmeter 
float potmeter()
{
    float out_1 = pot_1 * 2.0f;
    float out_2 = out_1 - 1.0f;
                
    dirpin_1.write(out_2 < 0);
    dirpin_2.write(out_2 < 0);
    
    pwmpin_1 = fabs (out_2); // Has to be positive value
    pwmpin_2 = fabs (out_2);
        
    return out_2;
}

// Motor calibration
// To calibrate the motor angle to some mechanical boundaries
// Kenneth mee bezig 

// EMG calibration
// To calibrate the EMG signal to some boundary values

// Send EMG to HIDScope
void sample() // Attach this to a ticker! 
{
    scope.set(0, counts); // send counts to channel 1 (=0)
    // scope.set(1, velocity_encoder() ); // sent EMG 2 to channel 2 (=1)
    
    // Ensure that enough channels are available (HIDScope scope(2))
    scope.send(); // Send all channels to the PC at once
}

// Start
// To move the robot to the starting position: middle
void start() 
{
    // move to middle
    }

// Operating mode
// To control the robot with EMG signals

// Processing EMG 
// To process the raw EMG to a usable value, with filters etc
// Simon en Kees mee bezig 

// Demo mode
// To control the robot with a written code and write 'HELLO'
// Voor het laatst bewaren

// Emergency mode
// To shut down the robot after an error etc 

// Main function
// -----------------------------------------------------------------------------
int main()
{
    pc.baud(115200); // For TeraTerm, the baudrate, set also in TeraTerm itself! 
    pc.printf("Start code\r\n"); // To check if the program starts 
    pwmpin_1.period_us(60); // Needed for PWM, not exactly known why
    
    while(true){
        
        counts = encoder();
        float out_2 = potmeter();
        sample(); 
        
        pc.printf("potmeter value = %f ", out_2);
        pc.printf("counts = %i\r\n", counts); 
        
        /* if (button_motorcal == true){
            // execute motor calibration
            }
        elseif {
            // remain in waiting mode 
            break;
            }
            */
        
        wait(0.01); // loop the while loop every 0.01 seconds
        }
        
}