Dynamics ident of the system
Dependencies: FastPWM HIDScope MODSERIAL QEI mbed
Fork of Berekenen_motorhoek by
main.cpp
- Committer:
- sjoerdbarts
- Date:
- 2016-10-28
- Revision:
- 7:167b6dfdefee
- Parent:
- 6:ee243782bf51
File content as of revision 7:167b6dfdefee:
#include "mbed.h" #include "FastPWM.h" #include "HIDScope.h" #include "QEI.h" #include "MODSERIAL.h" #define SERIAL_BAUD 115200 MODSERIAL pc(USBTX,USBRX); // Set button pins InterruptIn button1(PTB9); InterruptIn button2(PTA1); // Set AnalogIn potmeters AnalogIn pot1(A3); AnalogIn pot2(A4); // Set motor Pinouts DigitalOut motor1_dir(D4); FastPWM motor1_pwm(D5); DigitalOut motor2_dir(D7); FastPWM motor2_pwm(D6); // Set LED pins DigitalOut led(LED_RED); // Set HID scope HIDScope scope(3); // Set Encoders for motors QEI Motor1_ENC_CW(D11,D10,NC,32); QEI Motor1_ENC_CCW(D10,D11,NC,32); QEI Motor2_ENC_CW(D12,D13,NC,32); QEI Motor2_ENC_CCW(D13,D12,NC,32); // Constants volatile double pwm = 0.0; volatile double pwm_new = 0; volatile double timer = 0.00000000; volatile bool button1_value = false; volatile bool button2_value = false; // Ticker Ticker SinTicker; Ticker PotControlTicker; Ticker SendZerosTicker; void SwitchLed() { led = not led; } void button1_switch() { button1_value = not button1_value; } void button2_switch() { button2_value = not button2_value; } void SendZeros() { int zero=0; scope.set(0,zero); scope.set(1,zero); scope.set(2,zero); scope.send(); } void SetMotor(int motor_number, double MotorValue) { // Given -1<=motorValue<=1, this sets the PWM and direction // bits for motor 1. Positive value makes motor rotating // clockwise. motorValues outside range are truncated to // within range if (motor_number == 1) { if (MotorValue >=0) { motor1_dir=1; } else { motor1_dir=0; } if (fabs(MotorValue)>1){ motor1_pwm.write(1); } else { motor1_pwm.write(abs(MotorValue)); } } else { if (MotorValue >=0) { motor2_dir=1; } else { motor2_dir=0; } if (fabs(MotorValue)>1){ motor2_pwm.write(1); } else { motor2_pwm.write(abs(MotorValue)); } } } double GetPosition(int motor_number) { const int COUNTS_PER_REV = 8400; const float DEGREES_PER_PULSE = 8400 / 360; if (motor_number == 1) { int countsCW = Motor1_ENC_CW.getPulses(); int countsCCW= Motor1_ENC_CCW.getPulses(); int net_counts = countsCW-countsCCW; double Position=(net_counts*360.0f)/COUNTS_PER_REV; return Position; } else { int countsCW = Motor2_ENC_CW.getPulses(); int countsCCW= Motor2_ENC_CCW.getPulses(); int net_counts = countsCW-countsCCW; double Position=(net_counts*360.0f)/COUNTS_PER_REV; return Position; } } void MultiSin_motor1() { double f1 = 0.2f; double f2 = 0.4f; double f3 = 0.8f; double f4 = 1.6f; double f5 = 3.2f; double f6 = 6.3f; double f7 = 12.6f; double f8 = 25.0f; double f9 = 50.0f; double f10 = 100.0f; const double pi = 3.141592653589793238462; // read encoder double motor1_position = GetPosition(1); // set HIDSCOPE values, position, pwm*volt scope.set(0, motor1_position); scope.set(1, pwm_new); scope.set(2, timer); scope.send(); // sent HIDScope values // calculate the new multisin double multisin = sin(2*pi*f1*timer)+sin(2*pi*f2*timer)+sin(2*pi*f3*timer)+sin(2*pi*f4*timer)+sin(2*pi*f5*timer)+sin(2*pi*f6*timer)+sin(2*pi*f7*timer)+sin(2*pi*f8*timer)+sin(2*pi*f9*timer)+sin(2*pi*f10*timer); // devide by the amount of sin waves pwm_new=multisin / 10.0f; // write the motors SetMotor(1, pwm_new); // Increase the timer timer = timer + 0.001f; if (timer >= (10.00000f)) { SetMotor(1, 0); SinTicker.detach(); timer = 0; } } void MultiSin_motor2() { double f1 = 0.2f; double f2 = 0.4f; double f3 = 0.8f; double f4 = 1.6f; double f5 = 3.2f; double f6 = 6.3f; double f7 = 12.6f; double f8 = 25.0f; double f9 = 50.0f; double f10 = 100.0f; const double pi = 3.141592653589793238462; // read encoder double motor2_position = GetPosition(2); // set HIDSCOPE values, position, pwm*volt scope.set(0, motor2_position); scope.set(1, pwm_new); scope.set(2, timer); scope.send(); // calculate the new multisin double multisin = sin(2*pi*f1*timer)+sin(2*pi*f2*timer)+sin(2*pi*f3*timer)+sin(2*pi*f4*timer)+sin(2*pi*f5*timer)+sin(2*pi*f6*timer)+sin(2*pi*f7*timer)+sin(2*pi*f8*timer)+sin(2*pi*f9*timer)+sin(2*pi*f10*timer); // devide by the amount of sin waves pwm_new=multisin / 10.0f; // write the motors SetMotor(2, pwm_new); // Increase the timer timer = timer + 0.001f; if (timer >= (10.0000f)) { SetMotor(2, 0); SinTicker.detach(); timer = 0; } } void PotControl() { double Motor1_Value = (pot1.read() - 0.5)/2.0; double Motor2_Value = (pot2.read() - 0.5)/2.0; //pc.printf("\r\n Motor value 1: %f \r\n",Motor1_Value); //pc.printf("\r\n Motor value 2: %f \r\n",Motor2_Value); scope.set(0, Motor1_Value); scope.set(1, Motor2_Value); scope.send(); // Write the motors SetMotor(1, Motor1_Value); SetMotor(2, Motor2_Value); } void SinControl1() { SinTicker.attach(&MultiSin_motor1, 0.001); } void SinControl2() { SinTicker.attach(&MultiSin_motor2, 0.001); } void HIDScope_Send() { scope.send(); } int main() { pc.baud(SERIAL_BAUD); pc.printf("\r\n ***THERMONUCLEAR WARFARE HAS COMMENCED*** \r\n"); // Set LED led=0; Ticker LedTicker; LedTicker.attach(SwitchLed,0.5f); // Interrupt for buttons button1.fall(button1_switch); button2.fall(button2_switch); pc.printf("\r\n ***************** \r\n"); pc.printf("\r\n Press button 1 to start positioning the arms \r\n"); pc.printf("\r\n Make sure both potentiometers are positioned halfway! \r\n"); pc.printf("\r\n ***************** \r\n"); while (button1_value == false) { // just wait } pc.printf("\r\n ***************** \r\n"); pc.printf("\r\n Use potentiometers to control the motor arms \r\n"); pc.printf("\r\n Pot 1 for motor 1 \r\n"); pc.printf("\r\n Pot 2 for motor 2 \r\n"); pc.printf("\r\n ***************** \r\n"); pc.printf("\r\n Press button 2 to start the rest of the program \r\n"); pc.printf("\r\n ***************** \r\n"); // Start potmeter control PotControlTicker.attach(&PotControl,0.01f); while (button2_value == false) { // just wait } // When button is pressed detach PotControl and sendzeros to HIDScope PotControlTicker.detach(); // Reset the motor PWM values to zero SetMotor(1, 0); SetMotor(2, 0); // wait a bit to make sure all movement is gone wait(0.5); // Reset the QUE encoder values so that the currect position is nog the 0 position Motor1_ENC_CW.reset(); Motor1_ENC_CCW.reset(); Motor2_ENC_CW.reset(); Motor2_ENC_CCW.reset(); // Flush the HIDScope with zeros SendZerosTicker.attach(&SendZeros,0.001); wait(1); SendZerosTicker.detach(); // Program startup pc.printf("\r\n Starting multisin on motor 1 in: \r\n"); pc.printf("\r\n 10 \r\n"); wait(5); pc.printf("\r\n 5 \r\n"); wait(1); pc.printf("\r\n 4 \r\n"); wait(1); pc.printf("\r\n 3 \r\n"); wait(1); pc.printf("\r\n 2 \r\n"); wait(1); pc.printf("\r\n 1 \r\n"); wait(1); pc.printf("\r\n 0 \r\n"); pc.printf("\r\n Wait for the signal to complete \r\n"); pc.printf("\r\n Press button 2 too continue afterwards \r\n"); SinControl1(); while (button2_value == true) { // just wait } // Flush the HIDScope with zeros SendZerosTicker.attach(&SendZeros,0.001); wait(1); SendZerosTicker.detach(); // Start multisine om motor 2 routine pc.printf("\r\n Starting multisin on motor 2 in: \r\n"); pc.printf("\r\n 10 \r\n"); wait(5); pc.printf("\r\n 5 \r\n"); wait(1); pc.printf("\r\n 4 \r\n"); wait(1); pc.printf("\r\n 3 \r\n"); wait(1); pc.printf("\r\n 2 \r\n"); wait(1); pc.printf("\r\n 1 \r\n"); wait(1); pc.printf("\r\n 0 \r\n"); SinControl2(); wait(23); pc.printf("\r\n Program Finished, press reset to restart \r\n"); while (true) {} }