As used for motor testing May / June 2016

Dependencies:   iC_MU mbed-rtos mbed

Fork of SweptSine by Vitec Group

main.cpp

Committer:
acodd
Date:
2016-06-08
Revision:
3:463edcfc09c5
Parent:
2:be3b6a72f823

File content as of revision 3:463edcfc09c5:

#include "mbed.h"
#include "iC_MU.h"
#include "rtos.h"
#include "MyAxis.h"

// iC-MU Encoder Objects
iC_MU tilt_Motor(p5,p6,p7,p8);
iC_MU TiltPos(p5,p6,p7,p11);
iC_MU pan_Motor(p5,p6,p7,p12);
iC_MU PanPos(p5,p6,p7,p13);

// Tilt Motor
PwmOut Tilt_Motor_PWM(p23);                     // Purple wire
DigitalOut Tilt_Motor_Direction(p24);           // Yellow wire
// Pan Motor
PwmOut Pan_Motor_PWM(p21);                      // Purple wire
DigitalOut Pan_Motor_Direction(p22);            // Yellow wire

// Serial objects
Serial pc (USBTX,USBRX);

int ServiceKeyboard();


// Global Variables for sinusoidrun
float run_time = 10.0;
float start_Hz = 1.0, stop_Hz = 2.0;
extern float amplitude;
bool sinusoid = 0;
extern float a;
extern float Hz;

// Global variables for auto gearbox calculator
bool ratiocalc = 0;


// External variables
extern int LasticMuPosition;

// Functions
void PanAxisInitialise();
void TiltAxisInitialise();
void AxisControlLoop(MyAxis& Axis);
void Pan_Counter(void const *args);

void RunLoop(void const *args)
{
    AxisControlLoop(Pan);
    AxisControlLoop(Tilt);
}

int main()
{
    // Initalise Pan Velocity loop RtosTimer thread
    //Startme();
    pc.baud(921600);
    pc.printf("\n\r AH-10 Motor Scope Tool \n\r");
    pc.printf("\n\r Press q for menu \n\r");
    PanAxisInitialise();
    TiltAxisInitialise();
    RtosTimer RunLoop_timer(RunLoop, osTimerPeriodic);
    RunLoop_timer.start(1);             // Run at 1kHz
    
    
    // Initalise Pan counter RtosTimer thread
    RtosTimer Pan_Counter_timer(Pan_Counter, osTimerPeriodic);
    Pan_Counter_timer.start(1);


    while(1) {

        if(pc.readable()) {
            ServiceKeyboard();
        }

        if (sinusoid) {
            sinusoid = false;

            // Prompt the user to enter the test parameters
            pc.printf("\n\r Enter Start frequency (Hz): ");
            pc.scanf("%f", &start_Hz);
            pc.printf("%f",start_Hz);

            pc.printf("\n\r Enter Stop frequency (Hz): ");
            pc.scanf("%f", &stop_Hz);
            pc.printf("%f",stop_Hz);

            pc.printf("\n\r Enter Amplitude (encoder counts): ");
            pc.scanf("%f", &amplitude);
            pc.printf("%f",amplitude);

            pc.printf("\n\r Enter Time (s): ");
            pc.scanf("%f", &run_time);
            pc.printf("%f",run_time);

            pc.printf("\n\n\r Press any key to start test...");
            pc.getc();

            a = (stop_Hz - start_Hz) / (run_time * 1000);  // This is the incrementor
            pc.printf("\n\n\n\r ********************************************* \n\n\n\r *************************************************");
            // pc.printf("\n\n\r Time (ms),input,output,frequency (Hz)");
            pc.printf("\n\n\r Time (ms),Motor,System,frequency (Hz x 100),Demand");

            Pan.start_SysPos = Pan.System_position;
            Pan.start_MotPos = Pan.Motor_position;

            Tilt.start_SysPos = Tilt.System_position;
            Tilt.start_MotPos = Tilt.Motor_position;

            // Set the test running#
            if (Pan.jog > 0){
            Pan.sinusoidrun = 1;
            }
            if (Tilt.jog > 0) {
            Tilt.sinusoidrun = 1;
            }
        }

        if (ratiocalc) {
            ratiocalc = false;

            Pan.start_SysPos = Pan.System_position;
            Pan.start_MotPos = Pan.Motor_position;

            Pan.calcmyratio = true;

            Tilt.start_SysPos = Tilt.System_position;
            Tilt.start_MotPos = Tilt.Motor_position;

            Tilt.calcmyratio = true;
        }
    }

}