juggler_team / Mbed 2 deprecated juggler_mbed_position_control

Juggler Position Control Parsing

Dependencies:   MODSERIAL mbed Servo

Fork of juggler_mbed_position_control by Robert Katzschmann

main.cpp@8:b7a08dbbe1f9, 2017-01-16 (annotated)

Committer:
Symplectomorphism
Date:
Mon Jan 16 21:48:18 2017 +0000
Revision:
8:b7a08dbbe1f9
Parent:
7:7f6490ede6eb 
mbed command code finished

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rkk 0:1b69f8402884 1 #include "mbed.h"
rkk 0:1b69f8402884 2 #include "MODSERIAL/MODSERIAL.h"
rkk 0:1b69f8402884 3 #include "SerialComm/SerialComm.h"
Symplectomorphism 1:153ce28ac5ed 4 #include <stdlib.h>
Symplectomorphism 1:153ce28ac5ed 5 #include <stdio.h>
Symplectomorphism 1:153ce28ac5ed 6 #include <math.h>
rkk 0:1b69f8402884 7
Symplectomorphism 3:dcaf3c1e4ed6 8 #define USHRT_MAX 65535
Symplectomorphism 3:dcaf3c1e4ed6 9 #define POWER_OFF 32768
Symplectomorphism 3:dcaf3c1e4ed6 10 #define BACKWARD_MAX 0
Symplectomorphism 3:dcaf3c1e4ed6 11 #define FORWARD_MAX USHRT_MAX
Symplectomorphism 8:b7a08dbbe1f9 12 #define SEND_PERIOD_TIME_S 0.002
rkk 0:1b69f8402884 13
rkk 0:1b69f8402884 14 MODSERIAL pcSerial(USBTX, USBRX);
rkk 0:1b69f8402884 15
rkk 0:1b69f8402884 16 // Initialize a pins t//o perform analog and digital output fucntions
Symplectomorphism 1:153ce28ac5ed 17 PwmOut led1(LED1);
Symplectomorphism 3:dcaf3c1e4ed6 18 DigitalOut led2(LED2);
Symplectomorphism 3:dcaf3c1e4ed6 19 DigitalOut led3(LED3);
Symplectomorphism 5:2c84869b2edb 20 DigitalOut led4(LED4);
Symplectomorphism 3:dcaf3c1e4ed6 21
Symplectomorphism 3:dcaf3c1e4ed6 22 PwmOut motorPwm(p26);
Symplectomorphism 3:dcaf3c1e4ed6 23 AnalogOut motorAna(p18);
Symplectomorphism 3:dcaf3c1e4ed6 24 DigitalOut motorEnable(p12);
Symplectomorphism 1:153ce28ac5ed 25 Ticker aTimer;
Symplectomorphism 5:2c84869b2edb 26 AnalogIn motorSpeed(p15);
Symplectomorphism 3:dcaf3c1e4ed6 27 AnalogIn potentiometer(p20);
Symplectomorphism 3:dcaf3c1e4ed6 28 DigitalIn potentiometerEnable(p21);
rkk 0:1b69f8402884 29
rkk 0:1b69f8402884 30 SerialComm serialComm(&pcSerial);
rkk 0:1b69f8402884 31
Symplectomorphism 1:153ce28ac5ed 32 bool readyToSendNext;
Symplectomorphism 8:b7a08dbbe1f9 33 void timerFcn()
Symplectomorphism 8:b7a08dbbe1f9 34 {
Symplectomorphism 8:b7a08dbbe1f9 35 readyToSendNext = true;
Symplectomorphism 8:b7a08dbbe1f9 36 }
rkk 0:1b69f8402884 37
rkk 0:1b69f8402884 38 int main(void)
rkk 0:1b69f8402884 39 {
Symplectomorphism 4:a3720ed072d2 40 // DEFINE the operation modes
Symplectomorphism 5:2c84869b2edb 41 bool messageMode = true;
Symplectomorphism 7:7f6490ede6eb 42 bool analogMode = false;
Symplectomorphism 4:a3720ed072d2 43 led1 = 0.0f;
Symplectomorphism 3:dcaf3c1e4ed6 44 led2 = 0;
Symplectomorphism 3:dcaf3c1e4ed6 45 led3 = 0;
Symplectomorphism 5:2c84869b2edb 46 led4 = 0;
Symplectomorphism 3:dcaf3c1e4ed6 47 motorEnable = 0; // turn off motor
Symplectomorphism 3:dcaf3c1e4ed6 48 motorAna.write_u16(POWER_OFF); // set motor pwm to zero
Symplectomorphism 7:7f6490ede6eb 49 motorPwm.period_us(800);
Symplectomorphism 4:a3720ed072d2 50 motorPwm = 0.5f;
Symplectomorphism 4:a3720ed072d2 51
Symplectomorphism 4:a3720ed072d2 52 uint16_t motorCommandReceived = POWER_OFF;
Symplectomorphism 7:7f6490ede6eb 53 uint16_t motorCommandLast = POWER_OFF;
Symplectomorphism 5:2c84869b2edb 54 uint16_t motorSpeedMeas = POWER_OFF;
Symplectomorphism 4:a3720ed072d2 55 float motorCommandFloat = 0.5f;
Symplectomorphism 1:153ce28ac5ed 56 pcSerial.baud(115200);
rkk 0:1b69f8402884 57 pcSerial.printf("Start!\n");
rkk 0:1b69f8402884 58
Symplectomorphism 1:153ce28ac5ed 59 readyToSendNext = false;
Symplectomorphism 8:b7a08dbbe1f9 60
Symplectomorphism 8:b7a08dbbe1f9 61 //aTimer.attach(&timerFcn, SEND_PERIOD_TIME_S ); // the address of the function to be attached (flip) and the interval (2 seconds)
Symplectomorphism 8:b7a08dbbe1f9 62
Symplectomorphism 1:153ce28ac5ed 63 while(1) {
Symplectomorphism 3:dcaf3c1e4ed6 64 if(serialComm.checkIfNewMessage()) {
Symplectomorphism 4:a3720ed072d2 65 // read message from PC in order to clear the buffer
Symplectomorphism 3:dcaf3c1e4ed6 66 motorCommandReceived = serialComm.getUnsignedShort();
Symplectomorphism 4:a3720ed072d2 67
Symplectomorphism 4:a3720ed072d2 68 // -- ENABLE OR DISABLE MOTOR --
Symplectomorphism 4:a3720ed072d2 69 motorEnable = potentiometerEnable; // set led2 value based on switch of potentiometer
Symplectomorphism 4:a3720ed072d2 70 led2 = motorEnable;
Symplectomorphism 4:a3720ed072d2 71
Symplectomorphism 4:a3720ed072d2 72 // -- PC OR POTENTIOMETER Control Mode--
Symplectomorphism 4:a3720ed072d2 73 if(messageMode) { //control through pc
Symplectomorphism 7:7f6490ede6eb 74 if(motorCommandLast != motorCommandReceived) {
Symplectomorphism 7:7f6490ede6eb 75 if(analogMode) { //analog
Symplectomorphism 7:7f6490ede6eb 76 motorAna.write_u16(motorCommandReceived);
Symplectomorphism 7:7f6490ede6eb 77 } else { //PWM
Symplectomorphism 7:7f6490ede6eb 78 motorCommandFloat = ((float) motorCommandReceived)/ 65535.0f;
Symplectomorphism 7:7f6490ede6eb 79 motorPwm.write( motorCommandFloat * 0.8f + 0.1f);
Symplectomorphism 7:7f6490ede6eb 80 //led1 = motorCommandFloat; // drive led1 brightness accordingly
Symplectomorphism 7:7f6490ede6eb 81 }
Symplectomorphism 4:a3720ed072d2 82 }
Symplectomorphism 4:a3720ed072d2 83 } else { //potentiometer controlled
Symplectomorphism 7:7f6490ede6eb 84 motorCommandReceived = potentiometer.read_u16();
Symplectomorphism 7:7f6490ede6eb 85 if(motorCommandLast != motorCommandReceived) {
Symplectomorphism 7:7f6490ede6eb 86 if(analogMode) { //analog
Symplectomorphism 7:7f6490ede6eb 87 motorAna.write_u16(motorCommandReceived);
Symplectomorphism 7:7f6490ede6eb 88 } else { //PWM
Symplectomorphism 7:7f6490ede6eb 89 motorCommandFloat = ((float) motorCommandReceived) / 65535.0f;
Symplectomorphism 7:7f6490ede6eb 90 motorPwm.write( motorCommandFloat * 0.8f + 0.1f);
Symplectomorphism 7:7f6490ede6eb 91 //led1 = motorCommandFloat; // drive led1 brightness accordingly
Symplectomorphism 7:7f6490ede6eb 92 }
Symplectomorphism 4:a3720ed072d2 93 }
Symplectomorphism 3:dcaf3c1e4ed6 94 }
Symplectomorphism 7:7f6490ede6eb 95 motorCommandLast = motorCommandReceived;
Symplectomorphism 2:b4aef96e0cd7 96 readyToSendNext = true;
Symplectomorphism 1:153ce28ac5ed 97 }
Symplectomorphism 4:a3720ed072d2 98
Symplectomorphism 3:dcaf3c1e4ed6 99 if(readyToSendNext == true) {
Symplectomorphism 8:b7a08dbbe1f9 100 //led4 = 1;
Symplectomorphism 5:2c84869b2edb 101 // reading in current speed
Symplectomorphism 5:2c84869b2edb 102 motorSpeedMeas = motorSpeed.read_u16();
Symplectomorphism 5:2c84869b2edb 103 //sending speed to host
Symplectomorphism 8:b7a08dbbe1f9 104 //serialComm.sendUnsignedShort(motorSpeedMeas);
Symplectomorphism 8:b7a08dbbe1f9 105
Symplectomorphism 8:b7a08dbbe1f9 106 serialComm.sendUnsignedShort(motorCommandReceived);
Symplectomorphism 5:2c84869b2edb 107
Symplectomorphism 3:dcaf3c1e4ed6 108 readyToSendNext = false;
Symplectomorphism 8:b7a08dbbe1f9 109 //led4 = 0;
Symplectomorphism 1:153ce28ac5ed 110 }
Symplectomorphism 3:dcaf3c1e4ed6 111
rkk 0:1b69f8402884 112 }
rkk 0:1b69f8402884 113 }