Signa-bot code for project BioRobotics, at University of Twente.

Dependencies:   mbed QEI MODSERIAL FastPWM ttmath Math

Motor_tryout.cpp

Committer:
Feike
Date:
2019-10-05
Revision:
12:2382468d36a4
Parent:
9:61bdf58f856e
Child:
13:18dd7a15603f

File content as of revision 12:2382468d36a4:

#include "mbed.h" 
#include "MODSERIAL.h"
#include "QEI.h"

// ALLE INPUT EN OUTPUT SIGNALEN
DigitalOut motor1_pwm(PTC2);    // aansturen motor 1, via poort PTC2
DigitalOut motor1_dir(PTC3);    // richting motor 1
DigitalOut motor2_pwm(PTA2);    // aansturen motor 2, via poort PTA2
DigitalOut motor2_dir(PTB23);   // richting motor 2
DigitalIn motor1_1(PTD1);       // data vanuit motor 1
DigitalIn motor1_2(PTD3);       // data vanuit motor 1
AnalogIn input(PTB3);           // input van ECG
MODSERIAL pc(USBTX, USBRX);
QEI Encoder(D12,D13,NC,64,QEI::X4_ENCODING);

// DE TICKERS, deze ticker
Ticker pulse;
int counts;
void pulseget()
    {
    counts = Encoder.getPulses();
    }

// DE MAIN FUNCTIE
int main(void)
    {   
    pc.baud(115200);
    char cc = pc.getc();
    pc.printf("In de main, check\r\n");
    pc.putc(cc);
    if (cc == 'm' or cc == 'm')
        {
        pc.printf("In de if, check\r\n");
        pc.baud(115200);
        int frequency_pwm = 10000; //10 kHz PWM
        PwmOut motor1_pwm(PTC2);
        PwmOut motor2_pwm(PTA2);
        motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
        motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
        while(true)
            {   
            pc.printf("In de while, check\r\n");
            
            // MOTOR 1 START
            int limit = 20000;
                for(int pct = 55 ; pct <= 70 ; pct ++)
                {
                motor1_pwm.write(pct/100.0); // write Duty Cycle
                pulse.attach(pulseget,1.0/100);
                pc.printf("Motor 1 speed %i and the count is %i\r\n", pct, counts);
                wait(1.0f);
                if(counts > limit)
                    {
                    break;
                    }
                }
            motor1_pwm.write(0.0f); // motor 1 off again
            pc.printf("Motor 1 off\r\n");
            
            // MOTOR TWEE START
            for(int pct = 0 ; pct <= 100 ; pct += 10)
                {
                pc.printf("Motor 2\r\n");
                motor2_pwm.write(pct/100.0); // write Duty Cycle
                wait(1.0f);
                }
            motor2_pwm.write(0.0f); // motor 2 off again
            pc.printf("Motor 2 off\r\n");
            }
        }
    }