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@5:2c84869b2edb, 2017-01-16 (annotated)

Committer:
Symplectomorphism
Date:
Mon Jan 16 03:47:11 2017 +0000
Revision:
5:2c84869b2edb
Parent:
4:a3720ed072d2
Child:
6:75d01138b015
reading in speed from robot

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