main.cpp

00001 #include "mbed.h"
00002 #include "FastPWM.h"
00003 #include "HIDScope.h"
00004 #include "QEI.h"
00005 #include "MODSERIAL.h"
00006 #define SERIAL_BAUD 115200
00007  
00008 MODSERIAL pc(USBTX,USBRX);
00009 // Set button pins
00010 InterruptIn button1(PTB9);
00011 InterruptIn button2(PTA1);
00012 
00013 // Set AnalogIn potmeters
00014 AnalogIn pot1(A3);
00015 AnalogIn pot2(A4);
00016 
00017 // Set motor Pinouts
00018 DigitalOut motor1_dir(D4);
00019 FastPWM motor1_pwm(D5);
00020 DigitalOut motor2_dir(D7);
00021 FastPWM motor2_pwm(D6);
00022 
00023 // Set LED pins
00024 DigitalOut led(LED_RED);
00025 
00026 // Set HID scope
00027 HIDScope    scope(2);
00028 
00029 // Set Encoders for motors
00030 QEI Motor1_ENC_CW(D11,D10,NC,32);
00031 QEI Motor1_ENC_CCW(D10,D11,NC,32);
00032 QEI Motor2_ENC_CW(D12,D13,NC,32);
00033 QEI Motor2_ENC_CCW(D13,D12,NC,32);
00034 
00035 // Constants
00036 volatile double pwm = 0.0;
00037 volatile double pwm_new = 0;
00038 volatile double timer = 0.0;
00039 volatile bool button1_value = false;
00040 volatile bool button2_value = false;
00041 
00042 // Ticker
00043 Ticker SinTicker;
00044 Ticker PotControlTicker;
00045 Ticker SendZerosTicker;
00046 
00047 void SwitchLed()
00048 {
00049     led = not led;
00050 }
00051 
00052 void button1_switch()
00053 {
00054     button1_value = not button1_value;
00055 }
00056 
00057 void button2_switch()
00058 {
00059     button2_value = not button2_value;
00060 }
00061 
00062 void SendZeros()
00063 {
00064     int zero=0;
00065     scope.set(0,zero);
00066     scope.set(1,zero);
00067     scope.send();
00068 }
00069 
00070 void SetMotor(int motor_number, double MotorValue)
00071 {
00072     // Given -1<=motorValue<=1, this sets the PWM and direction
00073     // bits for motor 1. Positive value makes motor rotating
00074     // clockwise. motorValues outside range are truncated to
00075     // within range
00076     if (motor_number == 1)
00077     {
00078         if (MotorValue >=0)
00079         {
00080             motor1_dir=1;
00081         }
00082         else
00083         {
00084             motor1_dir=0;
00085         }
00086         if (fabs(MotorValue)>1){
00087             motor1_pwm.write(1);
00088         }
00089         else
00090         {
00091             motor1_pwm.write(abs(MotorValue));
00092         }
00093     }
00094     else
00095     {  
00096         if (MotorValue >=0)
00097         {
00098             motor1_dir=1;
00099         }
00100         else
00101         {
00102             motor1_dir=0;
00103         }
00104         if (fabs(MotorValue)>1){
00105             motor1_pwm.write(1);
00106         }
00107         else
00108         {
00109             motor1_pwm.write(abs(MotorValue));
00110         }
00111     }
00112         
00113 }
00114 
00115 double GetPosition(int motor_number)
00116 {
00117     const int   COUNTS_PER_REV = 8400;
00118     const float DEGREES_PER_PULSE = 8400 / 360;
00119     if (motor_number == 1)
00120     {
00121         int countsCW = Motor1_ENC_CW.getPulses();
00122         int countsCCW= Motor1_ENC_CCW.getPulses();
00123         int net_counts = countsCW-countsCCW;
00124         double Position=(net_counts*360.0f)/COUNTS_PER_REV;
00125         return Position;
00126     }
00127     else
00128     {
00129         int countsCW = Motor2_ENC_CW.getPulses();
00130         int countsCCW= Motor2_ENC_CCW.getPulses();
00131         int net_counts = countsCW-countsCCW;
00132         double Position=(net_counts*360.0f)/COUNTS_PER_REV;
00133         return Position;
00134     }
00135 }
00136 
00137 void MultiSin_motor1()
00138 {
00139     double pwm = pwm_new;
00140     double f1 = 0.2f;
00141     double f2 = 1.0f;
00142     double f3 = 5.0f;
00143     double f4 = 20.0f;
00144     double f5 = 100.0f;
00145     const double pi = 3.141592653589793238462;
00146     // read encoder
00147     double motor1_position = GetPosition(1);
00148     // set HIDSCOPE values, position, pwm*volt
00149     scope.set(0, motor1_position);
00150     scope.set(1, pwm);
00151     // sent HIDScope values
00152     scope.send();
00153     // calculate the new multisin
00154     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);
00155     // devide by the amount of sin waves
00156     pwm_new=multisin / 5;
00157     // write the motors
00158     SetMotor(1, multisin);
00159     // Increase the timer
00160     timer = timer + 0.001;
00161     if (timer >= (20.0-0.001))
00162     {
00163         SinTicker.detach();
00164         timer = 0;
00165     }
00166 }
00167 
00168 void MultiSin_motor2()
00169 {      
00170     double pwm = pwm_new;
00171     double f1 = 0.2f;
00172     double f2 = 1.0f;
00173     double f3 = 5.0f;
00174     double f4 = 20.0f;
00175     double f5 = 100.0f;
00176     const double pi = 3.141592653589793238462;
00177     // read encoder
00178     double motor2_position = GetPosition(2);
00179     // set HIDSCOPE values, position, pwm*volt
00180     scope.set(0, motor2_position);
00181     scope.set(1, pwm);
00182     // sent HIDScope values
00183     scope.send();
00184     // calculate the new multisin
00185     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);
00186     // devide by the amount of sin waves
00187     pwm_new=multisin / 5;
00188     // write the motor
00189     SetMotor(2, multisin);
00190     // Increase the timer
00191     timer = timer + 0.001;
00192     if (timer >= (20.0-0.001))
00193     {
00194         SinTicker.detach();
00195         timer = 0;
00196     }
00197 }
00198 
00199 void PotControl()
00200 {
00201     double Motor1_Value = (pot1.read() - 0.5)/2.0;
00202     double Motor2_Value = (pot2.read() - 0.5)/2.0;
00203     //pc.printf("\r\n Motor value 1: %f \r\n",Motor1_Value);
00204     //pc.printf("\r\n Motor value 2: %f \r\n",Motor2_Value);
00205     scope.set(0, Motor1_Value);
00206     scope.set(1, Motor2_Value);
00207     scope.send();
00208     // Write the motors
00209     SetMotor(1, Motor1_Value);
00210     SetMotor(2, Motor2_Value);
00211 }
00212 
00213 void SinControl1()
00214 {
00215     SinTicker.attach(&MultiSin_motor1, 0.001);
00216 }
00217 
00218 void SinControl2()
00219 {
00220     SinTicker.attach(&MultiSin_motor2, 0.001);
00221 }
00222 
00223 void HIDScope_Send()
00224 {
00225     scope.send();
00226 }
00227 
00228 
00229 
00230 int main() {
00231     pc.baud(SERIAL_BAUD);
00232     pc.printf("\r\n ***THERMONUCLEAR WARFARE HAS COMMENCED*** \r\n");
00233     
00234     // Set LED
00235     led=0;
00236     Ticker LedTicker;
00237     LedTicker.attach(SwitchLed,0.5f);    
00238     
00239     // Interrupt for buttons
00240     button1.fall(button1_switch);
00241     button2.fall(button2_switch);
00242     
00243     pc.printf("\r\n ***************** \r\n");
00244     pc.printf("\r\n Press button 1 to start positioning the arms \r\n");
00245     pc.printf("\r\n Make sure both potentiometers are positioned halfway! \r\n");
00246     pc.printf("\r\n ***************** \r\n");
00247     while (button1_value == false)
00248     {
00249         // just wait
00250     }
00251     pc.printf("\r\n ***************** \r\n");
00252     pc.printf("\r\n Use potentiometers to control the motor arms \r\n");
00253     pc.printf("\r\n Pot 1 for motor 1 \r\n");
00254     pc.printf("\r\n Pot 2 for motor 2 \r\n");
00255     pc.printf("\r\n ***************** \r\n");
00256     pc.printf("\r\n Press button 2 to start the rest of the program \r\n");
00257     pc.printf("\r\n ***************** \r\n");
00258     // Start potmeter control
00259     PotControlTicker.attach(&PotControl,0.01f);
00260     
00261     while (button2_value == false)
00262     {
00263         // just wait   
00264     }
00265     PotControlTicker.detach();
00266     SendZerosTicker.attach(&SendZeros,0.001);
00267     
00268     // Program startup   
00269     pc.printf("\r\n Starting multisin on motor 1 in: \r\n");
00270     pc.printf("\r\n 10 \r\n");
00271     wait(5);
00272     pc.printf("\r\n 5 \r\n");
00273     wait(1);
00274     pc.printf("\r\n 4 \r\n");
00275     wait(1);
00276     pc.printf("\r\n 3 \r\n");
00277     wait(1);
00278     pc.printf("\r\n 2 \r\n");
00279     wait(1);
00280     pc.printf("\r\n 1 \r\n");
00281     wait(1);
00282     pc.printf("\r\n 0 \r\n");
00283     pc.printf("\r\n Wait for the signal to complete \r\n");
00284     pc.printf("\r\n Press button 2 too continue afterwards \r\n");
00285     SendZerosTicker.detach();
00286     SinControl1();
00287 
00288     
00289     while (button2_value == true)
00290     {
00291         // just wait   
00292     }
00293     
00294     
00295     SendZerosTicker.attach(&SendZeros,0.001);
00296     pc.printf("\r\n Starting multisin on motor 2 in: \r\n");
00297     pc.printf("\r\n 10 \r\n");
00298     wait(5);
00299     pc.printf("\r\n 5 \r\n");
00300     wait(1);
00301     pc.printf("\r\n 4 \r\n");
00302     wait(1);
00303     pc.printf("\r\n 3 \r\n");
00304     wait(1);
00305     pc.printf("\r\n 2 \r\n");
00306     wait(1);
00307     pc.printf("\r\n 1 \r\n");
00308     wait(1);
00309     pc.printf("\r\n 0 \r\n");
00310     SendZerosTicker.detach();
00311     SinControl2();
00312     wait(23);
00313     pc.printf("\r\n Program Finished, press reset to restart \r\n");
00314     while (true) {}
00315 }