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@3:dcaf3c1e4ed6, 2017-01-15 (annotated)

Committer:
Symplectomorphism
Date:
Sun Jan 15 23:58:28 2017 +0000
Revision:
3:dcaf3c1e4ed6
Parent:
2:b4aef96e0cd7
Child:
4:a3720ed072d2
added manual controller

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
rkk 0:1b69f8402884 8 #define NUM_FLOATS 10
Symplectomorphism 3:dcaf3c1e4ed6 9 #define USHRT_MAX 65535
Symplectomorphism 3:dcaf3c1e4ed6 10
Symplectomorphism 3:dcaf3c1e4ed6 11 #define USHRT_MIN 0
Symplectomorphism 3:dcaf3c1e4ed6 12 #define POWER_OFF 32768
Symplectomorphism 3:dcaf3c1e4ed6 13 #define BACKWARD_MAX 0
Symplectomorphism 3:dcaf3c1e4ed6 14 #define FORWARD_MAX USHRT_MAX
rkk 0:1b69f8402884 15
rkk 0:1b69f8402884 16
rkk 0:1b69f8402884 17 MODSERIAL pcSerial(USBTX, USBRX);
rkk 0:1b69f8402884 18
rkk 0:1b69f8402884 19 // Initialize a pins t//o perform analog and digital output fucntions
rkk 0:1b69f8402884 20 AnalogOut aout(p18);
Symplectomorphism 1:153ce28ac5ed 21 PwmOut led1(LED1);
Symplectomorphism 3:dcaf3c1e4ed6 22 DigitalOut led2(LED2);
Symplectomorphism 3:dcaf3c1e4ed6 23 DigitalOut led3(LED3);
Symplectomorphism 3:dcaf3c1e4ed6 24
Symplectomorphism 3:dcaf3c1e4ed6 25 PwmOut motorPwm(p26);
Symplectomorphism 3:dcaf3c1e4ed6 26 AnalogOut motorAna(p18);
Symplectomorphism 3:dcaf3c1e4ed6 27 DigitalOut motorEnable(p12);
Symplectomorphism 1:153ce28ac5ed 28 Ticker aTimer;
Symplectomorphism 3:dcaf3c1e4ed6 29 AnalogIn potentiometer(p20);
Symplectomorphism 3:dcaf3c1e4ed6 30 DigitalIn potentiometerEnable(p21);
rkk 0:1b69f8402884 31
rkk 0:1b69f8402884 32 //Serial pc(USBTX, USBRX);
rkk 0:1b69f8402884 33 float valueFloats[NUM_FLOATS];
rkk 0:1b69f8402884 34 float voltage;
rkk 0:1b69f8402884 35
rkk 0:1b69f8402884 36 SerialComm serialComm(&pcSerial);
rkk 0:1b69f8402884 37
Symplectomorphism 1:153ce28ac5ed 38 bool readyToSendNext;
rkk 0:1b69f8402884 39
Symplectomorphism 1:153ce28ac5ed 40 void timerFcn()
Symplectomorphism 1:153ce28ac5ed 41 {
Symplectomorphism 1:153ce28ac5ed 42 readyToSendNext = true;
Symplectomorphism 1:153ce28ac5ed 43 }
rkk 0:1b69f8402884 44
rkk 0:1b69f8402884 45
rkk 0:1b69f8402884 46 int main(void)
rkk 0:1b69f8402884 47 {
Symplectomorphism 3:dcaf3c1e4ed6 48 led2 = 0;
Symplectomorphism 3:dcaf3c1e4ed6 49 led3 = 0;
Symplectomorphism 3:dcaf3c1e4ed6 50 motorEnable = 0; // turn off motor
Symplectomorphism 3:dcaf3c1e4ed6 51 motorAna.write_u16(POWER_OFF); // set motor pwm to zero
Symplectomorphism 1:153ce28ac5ed 52
Symplectomorphism 3:dcaf3c1e4ed6 53 uint16_t motorCommandReceived = 0;
Symplectomorphism 3:dcaf3c1e4ed6 54 uint16_t motorCommand = motorCommandReceived;
Symplectomorphism 3:dcaf3c1e4ed6 55 uint16_t motorCommandToSend = 0;
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 2:b4aef96e0cd7 60 //aTimer.attach(&timerFcn, 0.002); // the address of the function to be attached (flip) and the interval (2 seconds)
Symplectomorphism 1:153ce28ac5ed 61
Symplectomorphism 1:153ce28ac5ed 62 while(1) {
Symplectomorphism 3:dcaf3c1e4ed6 63
Symplectomorphism 3:dcaf3c1e4ed6 64 if(serialComm.checkIfNewMessage()) {
Symplectomorphism 3:dcaf3c1e4ed6 65 //fReceived = serialComm.getFloat();
Symplectomorphism 3:dcaf3c1e4ed6 66 motorCommandReceived = serialComm.getUnsignedShort();
Symplectomorphism 3:dcaf3c1e4ed6 67 led2 = potentiometerEnable;
Symplectomorphism 3:dcaf3c1e4ed6 68
Symplectomorphism 3:dcaf3c1e4ed6 69 if(potentiometerEnable) {
Symplectomorphism 3:dcaf3c1e4ed6 70 motorEnable = 1; //CHANGE TO 1
Symplectomorphism 3:dcaf3c1e4ed6 71 led3 = 1;
Symplectomorphism 3:dcaf3c1e4ed6 72 } else {
Symplectomorphism 3:dcaf3c1e4ed6 73 motorEnable = 0;
Symplectomorphism 3:dcaf3c1e4ed6 74 led3 = 0;
Symplectomorphism 3:dcaf3c1e4ed6 75 //motorAna.write_u16(POWER_OFF);
Symplectomorphism 3:dcaf3c1e4ed6 76 }
Symplectomorphism 3:dcaf3c1e4ed6 77
Symplectomorphism 3:dcaf3c1e4ed6 78 motorCommand = potentiometer.read_u16();
Symplectomorphism 3:dcaf3c1e4ed6 79 led1 = potentiometer;
Symplectomorphism 3:dcaf3c1e4ed6 80 motorAna.write_u16(motorCommand);
Symplectomorphism 3:dcaf3c1e4ed6 81
Symplectomorphism 3:dcaf3c1e4ed6 82 //motorAna.write_u16(motorCommandReceived);
Symplectomorphism 3:dcaf3c1e4ed6 83 //motorAna.write_u16(USHRT_MAX);
Symplectomorphism 3:dcaf3c1e4ed6 84
Symplectomorphism 2:b4aef96e0cd7 85 readyToSendNext = true;
Symplectomorphism 1:153ce28ac5ed 86 }
Symplectomorphism 3:dcaf3c1e4ed6 87
Symplectomorphism 3:dcaf3c1e4ed6 88 if(readyToSendNext == true) {
Symplectomorphism 3:dcaf3c1e4ed6 89 motorCommandToSend = motorCommandReceived; // / 3.3f;
Symplectomorphism 1:153ce28ac5ed 90 //led1 = fToSend; // voltage larger than 0.5*3.3
Symplectomorphism 3:dcaf3c1e4ed6 91
Symplectomorphism 1:153ce28ac5ed 92 //i++;
Symplectomorphism 1:153ce28ac5ed 93 // led1 = (ftemp > 0.5f) ? !led1 : 0; // voltage larger than 0.5*3.3
Symplectomorphism 3:dcaf3c1e4ed6 94 serialComm.sendUnsignedShort(motorCommandToSend); // write it back
Symplectomorphism 3:dcaf3c1e4ed6 95 readyToSendNext = false;
Symplectomorphism 1:153ce28ac5ed 96 }
Symplectomorphism 3:dcaf3c1e4ed6 97
rkk 0:1b69f8402884 98 }
rkk 0:1b69f8402884 99 }