Thomas Burgers / Mbed 2 deprecated Samenwerking_Motor12

Samenwerking Groep 12

Dependencies:   Encoder MODSERIAL HIDScope mbed

Foo

main.cpp@22:d22381fe3b16, 2015-09-22 (annotated)

Committer:
ThomasBNL
Date:
Tue Sep 22 13:59:36 2015 +0000
Revision:
22:d22381fe3b16
Parent:
21:fb17e0b8924e
Child:
23:34e97668550f
hidscope implementeren

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ThomasBNL 10:e923f51f18c4 1 #include "mbed.h"
ThomasBNL 10:e923f51f18c4 2 #include "HIDScope.h"
ThomasBNL 10:e923f51f18c4 3 #include "encoder.h"
ThomasBNL 10:e923f51f18c4 4 #include "MODSERIAL.h"
ThomasBNL 10:e923f51f18c4 5
ThomasBNL 10:e923f51f18c4 6 //Motor 2
ThomasBNL 10:e923f51f18c4 7 DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
ThomasBNL 19:269f19917d80 8 PwmOut motor2speed(D5); // D5 is an input on the motor that controls the speed of motor number 2
ThomasBNL 19:269f19917d80 9 DigitalIn button(PTA4); // PTA4 is used as a button controller (0 when pressed and 1 when not pressed)
ThomasBNL 21:fb17e0b8924e 10 Encoder motor2(D13,D12,true); // The encoder is able to read and set position/speed values
ThomasBNL 19:269f19917d80 11 MODSERIAL pc(USBTX,USBRX); // This input is used to send data to the pc
ThomasBNL 19:269f19917d80 12 Ticker TickerController; // This adds a Ticker to the function +TickerController
ThomasBNL 22:d22381fe3b16 13 HIDScope scope(2);
ThomasBNL 10:e923f51f18c4 14 //AnalogIn potmeter2(A0); /NIEUW
ThomasBNL 13:a0113cf2fc5f 15
ThomasBNL 19:269f19917d80 16 // Defining constants
ThomasBNL 19:269f19917d80 17 double counts_per_revolution=4200;
ThomasBNL 13:a0113cf2fc5f 18 double degrees_per_turn=360;
ThomasBNL 13:a0113cf2fc5f 19 double counts_per_degree=counts_per_revolution/degrees_per_turn; //11.67 counts/degree
ThomasBNL 22:d22381fe3b16 20 const double motor2_Kp = 1; //controller gain which will be multiplied with the error (*how fast will the error be corrected)
ThomasBNL 21:fb17e0b8924e 21 const double reference = 400;
ThomasBNL 13:a0113cf2fc5f 22
ThomasBNL 19:269f19917d80 23 // Function P-controller
ThomasBNL 19:269f19917d80 24 double P(double error, const double Kp) //returns error * controller gain
ThomasBNL 13:a0113cf2fc5f 25 {
ThomasBNL 19:269f19917d80 26 return Kp*error;
ThomasBNL 13:a0113cf2fc5f 27 }
ThomasBNL 19:269f19917d80 28 // Function that calls the P-controller
ThomasBNL 21:fb17e0b8924e 29 double motor2_controller() // Void function that compares the current position with a reference value and outputs
ThomasBNL 19:269f19917d80 30 {
ThomasBNL 19:269f19917d80 31 double position=motor2.getPosition(); //current motor position
ThomasBNL 21:fb17e0b8924e 32 double error=reference-position;
ThomasBNL 21:fb17e0b8924e 33 pc.printf("ik doe het positie = %d en error =%d en reference=%d \r\n", position, error, reference);
ThomasBNL 21:fb17e0b8924e 34 return P(error,motor2_Kp);
ThomasBNL 20:4d128c3f1228 35 }
ThomasBNL 13:a0113cf2fc5f 36
ThomasBNL 19:269f19917d80 37 // MAIN
ThomasBNL 10:e923f51f18c4 38 int main()
ThomasBNL 10:e923f51f18c4 39 {
ThomasBNL 19:269f19917d80 40 pc.baud(9600); // baud rate at which the information is processed to the computer
ThomasBNL 21:fb17e0b8924e 41 //motor2.setPosition(0); // calls the current position of the motor zero
ThomasBNL 10:e923f51f18c4 42 while(true) {
ThomasBNL 10:e923f51f18c4 43 if (button.read() < 0.5) { //if button pressed
ThomasBNL 13:a0113cf2fc5f 44 motor2direction = 0; // zero is clockwise (front view)
ThomasBNL 19:269f19917d80 45 motor2speed = 0.5f; // motorspeed
ThomasBNL 10:e923f51f18c4 46 pc.printf("positie = %d \r\n", motor2.getPosition());
ThomasBNL 10:e923f51f18c4 47 } else { // If button is not pressed
ThomasBNL 21:fb17e0b8924e 48 double output_motor = motor2_controller();
ThomasBNL 21:fb17e0b8924e 49 while(output_motor>0){
ThomasBNL 21:fb17e0b8924e 50 motor2speed=output_motor;
ThomasBNL 21:fb17e0b8924e 51 motor2direction=1;
ThomasBNL 21:fb17e0b8924e 52 break;}
ThomasBNL 21:fb17e0b8924e 53 while(output_motor<0){
ThomasBNL 21:fb17e0b8924e 54 motor2speed=fabs(output_motor);
ThomasBNL 21:fb17e0b8924e 55 motor2direction=0;
ThomasBNL 21:fb17e0b8924e 56 break;}
ThomasBNL 22:d22381fe3b16 57 while(1){
ThomasBNL 21:fb17e0b8924e 58 pc.printf("else loop controller");
ThomasBNL 22:d22381fe3b16 59 scope.set(0,motor2.getPosition());}
ThomasBNL 20:4d128c3f1228 60 }
ThomasBNL 21:fb17e0b8924e 61
ThomasBNL 19:269f19917d80 62 // while (Error_position > 0) {
ThomasBNL 19:269f19917d80 63 // motor2direction = 0;
ThomasBNL 19:269f19917d80 64 // motor2speed=0.3f;}
ThomasBNL 19:269f19917d80 65 // while (Error_position < 0) {
ThomasBNL 19:269f19917d80 66 // motor2direction = 1;
ThomasBNL 19:269f19917d80 67 // motor2speed=0.3f;}
ThomasBNL 20:4d128c3f1228 68 // Piece of code that keeps the value of the position between the counts per revolution
ThomasBNL 13:a0113cf2fc5f 69 while ((motor2.getPosition()>counts_per_revolution) || (motor2.getPosition()<-counts_per_revolution)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
ThomasBNL 12:af428d56b4eb 70 {
ThomasBNL 10:e923f51f18c4 71 motor2.setPosition(0);
ThomasBNL 13:a0113cf2fc5f 72 pc.printf(" HE \r\n LLO \r\n WO \r\n RLD \r\n !!! \r\n FOO! \r\n");
ThomasBNL 10:e923f51f18c4 73 break;
ThomasBNL 12:af428d56b4eb 74 }
ThomasBNL 10:e923f51f18c4 75 }
ThomasBNL 12:af428d56b4eb 76 }