Lisa Meijer / Mbed 2 deprecated Switch_motoren_motoren

Met servo motor

Dependencies:   MODSERIAL QEI mbed

Fork of Switch_motoren_motoren by Lian Beenhakker

main.cpp@2:725188195139, 2016-10-28 (annotated)

Committer:
lisa96m
Date:
Fri Oct 28 07:44:01 2016 +0000
Revision:
2:725188195139
Parent:
1:52a95e4b5662 
Aansturing motoren met servo geimplementeerd;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LBeen 0:602d9d743817 1 #include "mbed.h"
LBeen 0:602d9d743817 2 #include "MODSERIAL.h"
LBeen 0:602d9d743817 3 #include "QEI.h"
LBeen 0:602d9d743817 4
LBeen 0:602d9d743817 5 MODSERIAL pc(USBTX, USBRX);
LBeen 0:602d9d743817 6
LBeen 0:602d9d743817 7 //Pinnen voor spieren
LBeen 0:602d9d743817 8 AnalogIn Spier1 (A0);
LBeen 0:602d9d743817 9 AnalogIn Spier2 (A1);
LBeen 0:602d9d743817 10 InterruptIn Spier3 (D2);
LBeen 0:602d9d743817 11
LBeen 0:602d9d743817 12 //Pinnen voor motor
LBeen 0:602d9d743817 13 DigitalOut motor1direct (D4);
LBeen 0:602d9d743817 14 DigitalOut motor2direct (D7);
LBeen 0:602d9d743817 15 PwmOut motor1pwm (D5);
LBeen 0:602d9d743817 16 PwmOut motor2pwm (D6);
LBeen 0:602d9d743817 17 //QEI Encoder1(D10, D11, NC, 32); // met encoder onthouden waar je bent
LBeen 0:602d9d743817 18 //QEI Encoder2(D12, D13, NC, 32); // met encoder onthouden waar je bent
LBeen 0:602d9d743817 19
lisa96m 2:725188195139 20 //Pin voor de servo
lisa96m 2:725188195139 21 PwmOut servo(D9);
lisa96m 2:725188195139 22
LBeen 0:602d9d743817 23 //Define variables
LBeen 0:602d9d743817 24 volatile int indrukken = 0;
LBeen 0:602d9d743817 25 volatile int i = 0;
LBeen 0:602d9d743817 26 volatile float MV = 0;
LBeen 0:602d9d743817 27 //const float maxVelocity = 8.4;
LBeen 0:602d9d743817 28 //const float MotorGain=8.4;
LBeen 0:602d9d743817 29
LBeen 1:52a95e4b5662 30
LBeen 1:52a95e4b5662 31 //Als straks de kalibratie af is, dan moet Spier1 > 0.4 en Spier2 >0.4 nog aangepast worden met boven threshold of niet
LBeen 1:52a95e4b5662 32
LBeen 0:602d9d743817 33 //Motorvalue is een waarde tussen -1 en 1 waarmee de motor een richting en een snelheid krijgt
LBeen 0:602d9d743817 34 float GetMotorValue() //We nemen aan dat je maar één spier tegelijkertijd kan aanspannen
LBeen 0:602d9d743817 35 {
LBeen 0:602d9d743817 36 pc.printf("\n\n\n");
LBeen 0:602d9d743817 37 if(Spier1 > 0.4f && Spier2 < 0.4f) //Spier komt boven de threshold uit en spier2 niet
LBeen 0:602d9d743817 38 {
LBeen 0:602d9d743817 39 MV = 0.5;
LBeen 0:602d9d743817 40 pc.printf("Spier 1 is aangespannen\r\n");
LBeen 0:602d9d743817 41 }
LBeen 0:602d9d743817 42 else if(Spier1 < 0.4f && Spier2 > 0.4f)
LBeen 0:602d9d743817 43 {
LBeen 0:602d9d743817 44 MV = -0.5;
LBeen 0:602d9d743817 45 pc.printf("Spier 2 is aangespannen\r\n");
LBeen 0:602d9d743817 46 }
LBeen 0:602d9d743817 47 else if(Spier1 <0.4f && Spier2 <0.4f)
LBeen 0:602d9d743817 48 {
LBeen 0:602d9d743817 49 MV = 0;
LBeen 0:602d9d743817 50 pc.printf("Geen spier is aangespannen\r\n");
LBeen 0:602d9d743817 51 }
LBeen 0:602d9d743817 52 else
LBeen 0:602d9d743817 53 {
LBeen 0:602d9d743817 54 MV = 0;
LBeen 0:602d9d743817 55 pc.printf("Beide spieren zijn aangespannen\r\n");
LBeen 0:602d9d743817 56 }
LBeen 0:602d9d743817 57 pc.printf("de motorvalue is %f\n\n\r",MV);
LBeen 0:602d9d743817 58 return MV;
LBeen 0:602d9d743817 59 }
LBeen 0:602d9d743817 60
LBeen 0:602d9d743817 61 //Aan de hand van de motorvalue wordt de motor aangezet
LBeen 0:602d9d743817 62 void SetMotor1(float MV)
LBeen 0:602d9d743817 63 {
LBeen 0:602d9d743817 64 //Given -1<=motorValue<=1, this sets the PWM and direction
LBeen 0:602d9d743817 65 // bits for motor 1. Positive value makes motor rotating
LBeen 0:602d9d743817 66 // clockwise. motorValues outside range are truncated to
LBeen 0:602d9d743817 67 // within range
LBeen 0:602d9d743817 68 if (MV >=0)
LBeen 0:602d9d743817 69 {
LBeen 0:602d9d743817 70 motor1direct = 1;
LBeen 0:602d9d743817 71 pc.printf("motordirect=1\n\r"); //De motor draait in positieve richting, tegen de klok in
LBeen 0:602d9d743817 72 }
LBeen 0:602d9d743817 73 else
LBeen 0:602d9d743817 74 {
LBeen 0:602d9d743817 75 motor1direct = 0;
LBeen 0:602d9d743817 76 pc.printf("motordirect=0\n\r"); //De motor draait in negatieve richting, met de klok mee
LBeen 0:602d9d743817 77 }
LBeen 0:602d9d743817 78 if (fabs(MV)>1)
LBeen 0:602d9d743817 79 {
LBeen 0:602d9d743817 80 motor1pwm = 1;
LBeen 0:602d9d743817 81 pc.printf("motorpwm = 1\n\r"); //De snelheid waarmee de motor draait is maximaal, dus 1
LBeen 0:602d9d743817 82 }
LBeen 0:602d9d743817 83 else
LBeen 0:602d9d743817 84 {
LBeen 0:602d9d743817 85 motor1pwm = fabs(MV);
LBeen 0:602d9d743817 86 pc.printf("motorpwm = %f\n\n\r",fabs(MV)); //De snelheid waarmee de motor draait is de absolute waarde van de motorvalue
LBeen 0:602d9d743817 87 }
LBeen 0:602d9d743817 88 }
LBeen 0:602d9d743817 89
LBeen 0:602d9d743817 90 void SetMotor2(float MV)
LBeen 0:602d9d743817 91 {
LBeen 0:602d9d743817 92 //Given -1<=motorValue<=1, this sets the PWM and direction
LBeen 0:602d9d743817 93 // bits for motor 1. Positive value makes motor rotating
LBeen 0:602d9d743817 94 // clockwise. motorValues outside range are truncated to
LBeen 0:602d9d743817 95 // within range
LBeen 0:602d9d743817 96 if (MV >=0)
LBeen 0:602d9d743817 97 {
LBeen 0:602d9d743817 98 motor2direct = 1;
LBeen 0:602d9d743817 99 pc.printf("motordirect=1\n\r"); //De motor draait in positieve richting, tegen de klok in
LBeen 0:602d9d743817 100 }
LBeen 0:602d9d743817 101 else
LBeen 0:602d9d743817 102 {
LBeen 0:602d9d743817 103 motor2direct = 0;
LBeen 0:602d9d743817 104 pc.printf("motordirect=0\n\r"); //De motor draait in negatieve richting, met de klok mee
LBeen 0:602d9d743817 105 }
LBeen 0:602d9d743817 106 if (fabs(MV)>1)
LBeen 0:602d9d743817 107 {
LBeen 0:602d9d743817 108 motor2pwm = 1;
LBeen 0:602d9d743817 109 pc.printf("motorpwm = 1\n\r"); //De snelheid waarmee de motor draait is maximaal, dus 1
LBeen 0:602d9d743817 110 }
LBeen 0:602d9d743817 111 else
LBeen 0:602d9d743817 112 {
LBeen 0:602d9d743817 113 motor2pwm = fabs(MV);
LBeen 0:602d9d743817 114 pc.printf("motorpwm = %f\n\n\r",fabs(MV)); //De snelheid waarmee de motor draait is de absolute waarde van de motorvalue
LBeen 0:602d9d743817 115 }
LBeen 0:602d9d743817 116 }
LBeen 0:602d9d743817 117
lisa96m 2:725188195139 118 void SetMotor3(float MV)
lisa96m 2:725188195139 119 {
lisa96m 2:725188195139 120 if (MV > 0)
lisa96m 2:725188195139 121 {
lisa96m 2:725188195139 122 servo = 0.04;
lisa96m 2:725188195139 123 pc.printf("servo = 0.01\n\r"); //De servo draait maximaal rechtsom de klep dus open
lisa96m 2:725188195139 124 }
lisa96m 2:725188195139 125 else if (MV = 0)
lisa96m 2:725188195139 126 {
lisa96m 2:725188195139 127 servo = servo;
lisa96m 2:725188195139 128 pc.printf("servo = servo\n\r");
lisa96m 2:725188195139 129 }
lisa96m 2:725188195139 130 else (MV < 0)
lisa96m 2:725188195139 131 {
lisa96m 2:725188195139 132 servo = 0.08;
lisa96m 2:725188195139 133 pc.printf("servo = 0.15\n\r");
lisa96m 2:725188195139 134 }
lisa96m 2:725188195139 135 }
lisa96m 2:725188195139 136
LBeen 0:602d9d743817 137 void MeasureAndControl()
LBeen 0:602d9d743817 138 {
LBeen 0:602d9d743817 139 float MV = GetMotorValue();
LBeen 0:602d9d743817 140 switch(i)
LBeen 0:602d9d743817 141 {
LBeen 0:602d9d743817 142 case 0:
LBeen 0:602d9d743817 143 pc.printf("Motor 1\n\r");
LBeen 0:602d9d743817 144 SetMotor1(MV);
LBeen 0:602d9d743817 145 SetMotor2(0);
LBeen 0:602d9d743817 146 //SetMotor3(0);
LBeen 0:602d9d743817 147 break;
LBeen 0:602d9d743817 148 case 1:
LBeen 0:602d9d743817 149 pc.printf("Motor 2\n\r");
LBeen 0:602d9d743817 150 SetMotor1(0);
LBeen 0:602d9d743817 151 SetMotor2(MV);
LBeen 0:602d9d743817 152 //SetMotor3(0);
LBeen 0:602d9d743817 153 break;
LBeen 0:602d9d743817 154 case (2):
LBeen 0:602d9d743817 155 pc.printf("Motor 3\n\r");
LBeen 0:602d9d743817 156 SetMotor1(0);
LBeen 0:602d9d743817 157 SetMotor2(0);
LBeen 0:602d9d743817 158 //SetMotor3(MV);
LBeen 0:602d9d743817 159 break;
LBeen 0:602d9d743817 160 }
LBeen 0:602d9d743817 161 }
LBeen 0:602d9d743817 162
LBeen 0:602d9d743817 163 void count ()
LBeen 0:602d9d743817 164 {
LBeen 0:602d9d743817 165 indrukken ++;
LBeen 0:602d9d743817 166 pc.printf("Het knopje is %i x ingedrukt\n\r",indrukken);
LBeen 0:602d9d743817 167 i = indrukken%3;
LBeen 0:602d9d743817 168 }
LBeen 0:602d9d743817 169
LBeen 0:602d9d743817 170 int main()
LBeen 0:602d9d743817 171 {
LBeen 0:602d9d743817 172 //motorpwm.period(1.0/1000.0);
LBeen 0:602d9d743817 173 pc.baud(115200);
LBeen 0:602d9d743817 174 pc.printf("START\n\r");
LBeen 0:602d9d743817 175 Ticker motordraaien;
LBeen 0:602d9d743817 176 motordraaien.attach(MeasureAndControl,2);
LBeen 0:602d9d743817 177 Spier3.fall(count);
lisa96m 2:725188195139 178 servo.period(0.020) //servo periode 20ms
LBeen 0:602d9d743817 179 while(true){}
LBeen 0:602d9d743817 180 }
LBeen 0:602d9d743817 181