Diff: main.cpp

Revision:

6:ee243782bf51

Parent:

5:a9da7bc89b24

--- a/main.cpp	Mon Oct 17 14:25:12 2016 +0000
+++ b/main.cpp	Thu Oct 20 12:07:30 2016 +0000
@@ -1,230 +1,315 @@
 #include "mbed.h"
-#include "math.h"
-#include "iostream"
+#include "FastPWM.h"
+#include "HIDScope.h"
+#include "QEI.h"
+#include "MODSERIAL.h"
 #define SERIAL_BAUD 115200
-
-Serial pc(USBTX,USBRX);
-InterruptIn Button1(PTC6);          // DIT WORDT EEN ANDERE BUTTON
-
-// tickers
-Ticker TickerCaseBepalen;
-Ticker TickerVeranderenXY;
-Ticker TickerAfstandBerekenen;
-Ticker TickerMotorHoekBerekenen;
-Ticker TickerBerekenenBewegingsHoek;
-Ticker TickerFlipperBewegen;
-
-// in te voeren x en y waardes 
-volatile float x = 0.0;    
-volatile float y = 305.5; 
-volatile float vorigex = 0.0;
-volatile float vorigey = 0.0;
+ 
+MODSERIAL pc(USBTX,USBRX);
+// Set button pins
+InterruptIn button1(PTB9);
+InterruptIn button2(PTA1);
 
-// waardes
-volatile const float a = 50.0;       // de afstand van gekozen assenstelsel tot de armas (assenstelsel precies in het midden)   KEERTJE NAMETEN
-volatile const float Bar = 200.0;    // lengte van de armen                                                                     KEERTJE NAMETEN
-volatile const float pi = 3.14159265359; 
-volatile float VorigeHoek1 = 1.0/3.0*pi;     // begin hoek van 90 graden
-volatile float VorigeHoek2 = 1.0/3.0*pi; 
-volatile float Step = 1;    // EEN GOK VOOR HET STAPJE EN MOET DUS NOG AANGEPAST
-volatile float MaxHoek = pi - 30*pi/180;   // de hoek voordat arm het opstakel raakt (max hoek is 24.9 tussen arm en opstakel)
+// Set AnalogIn potmeters
+AnalogIn pot1(A3);
+AnalogIn pot2(A4);
 
-volatile float Dia1 = 0.0;
-volatile float Dia2 = 0.0;
-volatile float MotorHoek1 = 0.0;
-volatile float MotorHoek2 = 0.0;
-volatile float BewegingsHoek1 = 0.0;
-volatile float BewegingsHoek2 = 0.0;
-volatile bool Richting1 = true;     // cw = true, ccw = false
-volatile bool Richting2 = true;     
-volatile bool FlipperAan = false;   // als true dan flipper aanzetten
-bool BezigMetCalibreren = true;     // we beginnen met calibreren
-bool BeginnenMetFlippen = false;    // om de flipper aan te zetten
-bool BezigMetFlippen = false;       // om verandering in x en y tegen te houden tijdens het flippen. DIT WORDT VERANDERD IN SJOERDS DEEL (NAAM CONTROLEREN)
+// Set motor Pinouts
+DigitalOut motor1_dir(D4);
+FastPWM motor1_pwm(D5);
+DigitalOut motor2_dir(D7);
+FastPWM motor2_pwm(D6);
 
-// waardes die komen van Charlotte: true is aangespannen, false is niet aangespannen    (NAAM CONTROLEREN)
-volatile bool BicepsLinks = false;
-volatile bool BicepsRechts = true;
-volatile bool Been = false;
+// Set LED pins
+DigitalOut led(LED_RED);
 
-// nodig voor het switchstatement
-enum states {Flipper, Links, Rechts, Voor, Achter, GeenSignaal};
-states MyState = Links;
+// Set HID scope
+HIDScope    scope(2);
 
-// het bepalen van welke case we hebben
-void CaseBepalen () {
-    if (BicepsLinks==true && BicepsRechts==true && Been==true && BezigMetFlippen == false) {   
-        MyState = Flipper;      // flipper bewegen
-    }
-    else if (BicepsLinks==true && BicepsRechts==false && Been==false && BezigMetFlippen == false) {
-        MyState = Links;        // naar links bewegen
-    }
-    else if (BicepsLinks==false && BicepsRechts==true && Been==false && BezigMetFlippen == false) {
-        MyState = Rechts;       // naar rechts bewegen
-    }
-    else if (BicepsLinks==true && BicepsRechts==true && Been==false && BezigMetFlippen == false) {
-        MyState = Voor;         // naar voren bewegen
-    }
-    else if (BicepsLinks==false && BicepsRechts==true && Been==true && BezigMetFlippen == false) {
-        MyState = Achter;       // naar achter bewegen
-    }
-    else if (BicepsLinks==false && BicepsRechts==false && Been==false || BezigMetFlippen==true) {
-        MyState = GeenSignaal;
-    }
-}    
+// 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.0;
+volatile bool button1_value = false;
+volatile bool button2_value = false;
 
-// het berekenen van de nieuwe x en y en zorgen dat deze niet buiten het bereik komen
-void VeranderenXY () {
-        // bepalen hoe de x en y waardes gaan veranderen
-    switch(MyState) {
-        case Flipper :
-            BeginnenMetFlippen = true;     // we zijn aan het flipper dus geen andere dingen doen
-            break;
-        case Links : 
-            vorigex = x;    // ervoor zorgen dat als de motor te ver draait de x niet verder telt
-            x = x - Step;
-            break;
-        case Rechts :
-            vorigex = x;
-            x = x + Step;
-            break;
-        case Voor :
-            vorigey = y;    // ervoor zorgen dat als de motor te ver draait de y niet verder telt
-            y = y + Step;
-            break;
-        case Achter :
-            vorigey = y;
-            y = y - Step;
-            break;
-        case GeenSignaal :
-            break;
-        default :
-            pc.printf("Er is geen verandering in x en y\r\n");
-    }
-            
-    // Grenswaardes     LET OP: ARMEN MISSCHIEN GEBLOKKEERD DOOR BALK AAN DE BINNENKANT
-    if (x > 200) {
-        x = 200;
-    }
-    else if (x < -200) {
-        x = -200;
-    }
-    if (y > 306) {
-        y = 306;
-    }
-    else if (y < 50) {
-        y = 50;         // GOKJE, UITPROBEREN
-    }
-    pc.printf("x = %f, y = %f\r\n", x, y);
+// 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;
 }
 
-// functie om D1 en D2 te berekenen
-void AfstandBerekenen (){
-    float BV1 = sqrt(pow((a+x),2) + pow(y,2));  // diagonaal (afstand van armas tot locatie) berekenen
-    Dia1 = pow(BV1,2)/(2*BV1);     // berekenen van de afstand oorsprong tot diagonaal
-    
-    float BV2 = sqrt(pow((x-a),2) + pow(y,2)); // zelfde nog een keer doen maar nu voor de rechter arm 
-    Dia2 = pow(BV2,2)/(2*BV2); 
-    
-    //pc.printf("Dia1 = %f, Dia2 = %f\r\n", Dia1, Dia2);
+void SendZeros()
+{
+    int zero=0;
+    scope.set(0,zero);
+    scope.set(1,zero);
+    scope.send();
 }
 
-// functie om de motorhoek te berekenen
-void MotorHoekBerekenen (){
-    // eerst if loop doorlopen voor motor 1
-    if (x > -a) {
-        MotorHoek1 = pi - atan(y/(x+a)) - acos(Dia1/Bar);
-    }
-    else if (x > -a) {
-        MotorHoek1 = pi - (pi + atan(y/(x+a))) - acos(Dia1/Bar);
-    }
-    else {  // als x=-a
-        MotorHoek1 = 0.5f*pi - acos(Dia1/Bar);
-    }
-    // twee if loop doorlopen voor motor 2
-    if (x < a) {
-        MotorHoek2 = pi + atan(y/(x-a)) - acos(Dia2/Bar);
-    }
-    else if (x > a) {
-        MotorHoek2 = pi - (pi - atan(y/(x-a))) - acos(Dia2/Bar);
-    }
-    else {  // als x=a
-        MotorHoek2 = 0.5f*pi - acos(Dia1/Bar);
-    }
-    
-    // limiten (als de hoek te groot wordt, reset dan de hoek en de x en y waardes)
-    if (MotorHoek1 >= MaxHoek) {    // is de maximale hoek in radialen
-        MotorHoek1 = VorigeHoek1;
-        x = vorigex;
-        y = vorigey;
-    }    
-    if (MotorHoek2 >= MaxHoek) {
-        MotorHoek2 = VorigeHoek2;
-        x = vorigex;
-        y = vorigey;
+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));
+        }
     }
-    //pc.printf("MotorHoek1 = %f en MotorHoek2 = %f \r\n", MotorHoek1, MotorHoek2);
-}        
-
-// het verschil uitrekenen tussen waar we zijn en waar we heen willen       OVERLAP MET SJOERD
-void BerekenenBewegingsHoek () {
-    // Hoek 1: grootte beweging en richting
-    BewegingsHoek1 = VorigeHoek1 - MotorHoek1;
-    VorigeHoek1 = MotorHoek1;
-    if (BewegingsHoek1 > 0) {
-        Richting1 = false;      // ccw
-    }
-    else {
-        Richting1 = true;       // cw
+    else
+    {  
+        if (MotorValue >=0)
+        {
+            motor1_dir=1;
+        }
+        else
+        {
+            motor1_dir=0;
+        }
+        if (fabs(MotorValue)>1){
+            motor1_pwm.write(1);
+        }
+        else
+        {
+            motor1_pwm.write(abs(MotorValue));
+        }
     }
-    
-    // Hoek 2: grootte beweging en richting
-    BewegingsHoek2 = VorigeHoek2 - MotorHoek2;
-    VorigeHoek2 = MotorHoek2;
-    if (BewegingsHoek2 > 0) {
-        Richting2 = true;       // cw
-    }
-    else {
-        Richting2 = false;      // ccw
+        
+}
+
+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;
     }
-    //pc.printf("Beweging 1 = %f, Beweging 2 = %f\r\n", BewegingsHoek1, BewegingsHoek2);
-}  
-
-// als BeginnenMetFlippen aan staat voeren we deze functie uit. Zolang deze bezig is is BezigMetFlippen true
-void FlipperBewegen () {
-    if (BeginnenMetFlippen == true) {
-        // SJOERDS DEEL 
-    }
-    else {      // als BezigMetFlippen == false dan doe niks
+    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;
     }
 }
 
-// als de button ingedrukt wordt dan stoppen we met calibreren        
-void ButtonIndrukken () {
-        BezigMetCalibreren = false;
-}        
-        
+void MultiSin_motor1()
+{
+    double pwm = pwm_new;
+    double f1 = 0.2f;
+    double f2 = 1.0f;
+    double f3 = 5.0f;
+    double f4 = 20.0f;
+    double f5 = 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);
+    // sent HIDScope values
+    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);
+    // devide by the amount of sin waves
+    pwm_new=multisin / 5;
+    // write the motors
+    SetMotor(1, multisin);
+    // Increase the timer
+    timer = timer + 0.001;
+    if (timer >= (20.0-0.001))
+    {
+        SinTicker.detach();
+        timer = 0;
+    }
+}
+
+void MultiSin_motor2()
+{      
+    double pwm = pwm_new;
+    double f1 = 0.2f;
+    double f2 = 1.0f;
+    double f3 = 5.0f;
+    double f4 = 20.0f;
+    double f5 = 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);
+    // sent HIDScope values
+    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);
+    // devide by the amount of sin waves
+    pwm_new=multisin / 5;
+    // write the motor
+    SetMotor(2, multisin);
+    // Increase the timer
+    timer = timer + 0.001;
+    if (timer >= (20.0-0.001))
+    {
+        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 Nieuwe code uitproberen :) \r\n");
+    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);
     
-    // calibreren gebeurt hier, zodat de tickers nog niet lopen en dit dus geen rekenkracht kost en je niet gebonden bent aan tijden
-    // we gaan door met calibreren tot we de button indrukken
-    while (BezigMetCalibreren == true) {
-        // potmeter dingen aanpassen
-        
-        pc.printf("calibreren is bezig\r\n");
-        Button1.fall(&ButtonIndrukken);
+    while (button2_value == false)
+    {
+        // just wait   
+    }
+    PotControlTicker.detach();
+    SendZerosTicker.attach(&SendZeros,0.001);
+    
+    // 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");
+    SendZerosTicker.detach();
+    SinControl1();
+
+    
+    while (button2_value == true)
+    {
+        // just wait   
     }
     
-    TickerCaseBepalen.attach(&CaseBepalen, 1);                      // ticker om te bepalen welke case we gaan gebruiken
-    TickerVeranderenXY.attach(&VeranderenXY,1);                     // ticker voor het veranderen van de x en y waardes
-    TickerAfstandBerekenen.attach(&AfstandBerekenen,1);             // ticker om de waardes van dia1 en dia 2 te berekenen
-    TickerMotorHoekBerekenen.attach(&MotorHoekBerekenen,1);         // ticker om de motorhoek te berekenen
-    TickerBerekenenBewegingsHoek.attach(&BerekenenBewegingsHoek,1); // ticker om grote en richting vd beweging te berekenen
-    TickerFlipperBewegen.attach(&FlipperBewegen,1);                 // ticker om de flipper te laten bewegen
     
-    while (true) {
-        
-    }
+    SendZerosTicker.attach(&SendZeros,0.001);
+    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");
+    SendZerosTicker.detach();
+    SinControl2();
+    wait(23);
+    pc.printf("\r\n Program Finished, press reset to restart \r\n");
+    while (true) {}
 }