Biorobotics Project / Mbed OS Servocontrol_project

Servo position controlled by potvalue

Dependencies:   Servo

main.cpp@2:cef1384d2231, 2017-10-12 (annotated)

Committer:
MMartens
Date:
Thu Oct 12 14:35:20 2017 +0000
Revision:
2:cef1384d2231
Parent:
1:34cea73289c4
Child:
3:640691434e7e
Servo position controlled by potvalue

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MMartens 0:fe46ce7f03d3 1 /* This code controls the main functions of the servomotor. The servomotor needs an external power of 6 or 12 V and is connected to the PWMOut pin
MMartens 0:fe46ce7f03d3 2 External power can be either 4*1.5V AA batteries (6V), 8*1.5V AA batteries (12V) or an appropiate DC voltage source. It is important that ground of both the servo motor and mBed platform are the same.
MMartens 0:fe46ce7f03d3 3 Servo motor speed can be controlled by an external signal such as a potnetiometer or in context of this project an EMG signal.
MMartens 0:fe46ce7f03d3 4 */
MMartens 0:fe46ce7f03d3 5
MMartens 0:fe46ce7f03d3 6 #include <mbed.h>
MMartens 1:34cea73289c4 7 #include <Servo.h>
MMartens 1:34cea73289c4 8 #include <MODSERIAL.h>
MMartens 1:34cea73289c4 9 //#include <VarSpeedServo.h>
MMartens 1:34cea73289c4 10
MMartens 1:34cea73289c4 11 PwmOut PWM1(D7);
MMartens 1:34cea73289c4 12 MODSERIAL pc(USBTX,USBRX);
MMartens 1:34cea73289c4 13 AnalogIn pot(A1);
MMartens 1:34cea73289c4 14 Ticker potmeterreadout;
MMartens 1:34cea73289c4 15
MMartens 1:34cea73289c4 16 /* int PWMfreq(void) {
MMartens 1:34cea73289c4 17 PWM1.period(0.020); // set PWM period to 10 ms
MMartens 1:34cea73289c4 18 PWM1=0.5; // set duty cycle to 50%
MMartens 1:34cea73289c4 19 }
MMartens 1:34cea73289c4 20 */
MMartens 0:fe46ce7f03d3 21
MMartens 1:34cea73289c4 22 Servo myservo(D7);
MMartens 1:34cea73289c4 23 volatile float potvalue = 0.0;
MMartens 1:34cea73289c4 24 float range_servo = 0.001;
MMartens 1:34cea73289c4 25 float position_servo = 0.05;
MMartens 1:34cea73289c4 26 float servo_period = 0.01f;
MMartens 1:34cea73289c4 27
MMartens 1:34cea73289c4 28 void readpot(){
MMartens 1:34cea73289c4 29 potvalue = pot.read();
MMartens 1:34cea73289c4 30 //position = motor1.getPosition();
MMartens 1:34cea73289c4 31 pc.printf("reference position = %.2f\r", potvalue);
MMartens 1:34cea73289c4 32 myservo.calibrate(range_servo, 55.0);
MMartens 1:34cea73289c4 33 myservo = potvalue;
MMartens 1:34cea73289c4 34 //motorpid = PID(potvalue - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
MMartens 1:34cea73289c4 35 }
MMartens 1:34cea73289c4 36 // output PID controller is not yet tested.
MMartens 1:34cea73289c4 37
MMartens 1:34cea73289c4 38
MMartens 0:fe46ce7f03d3 39 int main() {
MMartens 0:fe46ce7f03d3 40 printf("Servo Calibration Controls:\n");
MMartens 0:fe46ce7f03d3 41 printf("1,2,3 - Position Servo (full left, middle, full right)\n");
MMartens 0:fe46ce7f03d3 42 printf("4,5 - Decrease or Increase range\n");
MMartens 0:fe46ce7f03d3 43
MMartens 1:34cea73289c4 44 float range_servo = 0.0008;
MMartens 1:34cea73289c4 45 float position_servo = 0.5;
MMartens 1:34cea73289c4 46
MMartens 1:34cea73289c4 47 pc.baud(9600);
MMartens 1:34cea73289c4 48 potmeterreadout.attach(readpot,0.05f);
MMartens 1:34cea73289c4 49 PWM1.period(servo_period);
MMartens 0:fe46ce7f03d3 50
MMartens 1:34cea73289c4 51 while(1){
MMartens 1:34cea73289c4 52 // Safety features to override potvalue
MMartens 1:34cea73289c4 53 /*switch(pc.getc()) {
MMartens 1:34cea73289c4 54 case '1': position_servo = 0.0; break;
MMartens 1:34cea73289c4 55 case '2': position_servo = 0.5; break;
MMartens 1:34cea73289c4 56 case '3': position_servo = 1.0; break;
MMartens 1:34cea73289c4 57 case '4': range_servo += 0.0001; break;
MMartens 1:34cea73289c4 58 case '5': range_servo -= 0.0001; break;
MMartens 0:fe46ce7f03d3 59 }
MMartens 1:34cea73289c4 60 */
MMartens 1:34cea73289c4 61 //printf("position = %.1f, range = +/-%0.4f\r", position_servo, range_servo);
MMartens 1:34cea73289c4 62 //myservo.calibrate(range_servo, 110.0);
MMartens 1:34cea73289c4 63 //myservo = position_servo;
MMartens 0:fe46ce7f03d3 64 }
MMartens 0:fe46ce7f03d3 65 }
MMartens 0:fe46ce7f03d3 66
MMartens 0:fe46ce7f03d3 67 /* Example code to control the servomotor by a potentiometer, similarly one could control the speed of the servomotor.
MMartens 0:fe46ce7f03d3 68 The speed can be controlled by using the library VarSpeedServo.h
MMartens 0:fe46ce7f03d3 69 By mapping the intensity of EMG signal to a range of values (e.g. 0:1:100) you can easily control the speed and position of the motor
MMartens 0:fe46ce7f03d3 70 */
MMartens 0:fe46ce7f03d3 71
MMartens 0:fe46ce7f03d3 72 /*
MMartens 0:fe46ce7f03d3 73 Servo servo_test; //initialize a servo object for the connected servo
MMartens 0:fe46ce7f03d3 74
MMartens 0:fe46ce7f03d3 75 int angle = 0;
MMartens 0:fe46ce7f03d3 76 int potentio = A0; // initialize the A0analog pin for potentiometer
MMartens 0:fe46ce7f03d3 77
MMartens 0:fe46ce7f03d3 78
MMartens 0:fe46ce7f03d3 79 void setup()
MMartens 0:fe46ce7f03d3 80 {
MMartens 0:fe46ce7f03d3 81 servo_test.attach(9); // attach the signal pin of servo to pin9 of arduino
MMartens 0:fe46ce7f03d3 82 }
MMartens 0:fe46ce7f03d3 83
MMartens 0:fe46ce7f03d3 84 void loop()
MMartens 0:fe46ce7f03d3 85 {
MMartens 0:fe46ce7f03d3 86 angle = analogRead(potentio); // reading the potentiometer value between 0 and 1023
MMartens 0:fe46ce7f03d3 87 angle = map(angle, 0, 1023, 0, 179); // scaling the potentiometer value to angle value for servo between 0 and 180)
MMartens 0:fe46ce7f03d3 88 servo_test.write(angle); //command to rotate the servo to the specified angle
MMartens 0:fe46ce7f03d3 89 delay(5);
MMartens 0:fe46ce7f03d3 90 }
MMartens 0:fe46ce7f03d3 91 */
MMartens 0:fe46ce7f03d3 92