Georgios Ardamerinos / Mbed 2 deprecated DynamicsProject

Phlebot's onboard code

Dependencies:   mbed

Fork of HelloWorld by Simon Ford

main.cpp@5:9f6830d1db7b, 2017-02-24 (annotated)

Committer:
gardamerinos
Date:
Fri Feb 24 06:18:49 2017 +0000
Revision:
5:9f6830d1db7b
Parent:
4:1070977b83e9
Child:
6:e69f8c6faebd
get serial input

Who changed what in which revision?

UserRevisionLine numberNew contents of line
simon 0:fb6bbc10ffa0 1 #include "mbed.h"
gardamerinos 4:1070977b83e9 2 #include <vector>
gardamerinos 5:9f6830d1db7b 3 #include <string>
gardamerinos 5:9f6830d1db7b 4 #include <sstream>
gardamerinos 5:9f6830d1db7b 5 #include <iostream>
simon 0:fb6bbc10ffa0 6
gardamerinos 5:9f6830d1db7b 7 //====================ALL DATA========================================
gardamerinos 4:1070977b83e9 8 vector<DigitalOut*> motors;
gardamerinos 5:9f6830d1db7b 9 vector<DigitalOut*> dirs;
gardamerinos 4:1070977b83e9 10 vector<int> steps;
gardamerinos 5:9f6830d1db7b 11 vector<int> currentPosition;
gardamerinos 5:9f6830d1db7b 12 vector<int> goalPosition;
gardamerinos 5:9f6830d1db7b 13 int positiveDirs [4] = {1,1,1,1};
gardamerinos 4:1070977b83e9 14
gardamerinos 5:9f6830d1db7b 15 //====================SERIAL COMMUNICATION=============================
gardamerinos 5:9f6830d1db7b 16 Serial pc(USBTX, USBRX);
gardamerinos 5:9f6830d1db7b 17
gardamerinos 5:9f6830d1db7b 18 //===================VARIABLES========================================
gardamerinos 5:9f6830d1db7b 19 int delayus = 100;
gardamerinos 5:9f6830d1db7b 20 int delayusOffset = delayus - 20;
gardamerinos 5:9f6830d1db7b 21 int stepsX = 5000;
gardamerinos 5:9f6830d1db7b 22 int stepsY = 5000;
gardamerinos 5:9f6830d1db7b 23 int stepsZ = 5000;
gardamerinos 5:9f6830d1db7b 24 int stepsRot = 100;
gardamerinos 5:9f6830d1db7b 25 string msg;
gardamerinos 5:9f6830d1db7b 26
gardamerinos 5:9f6830d1db7b 27 //======================FUNCTIONS=======================================
gardamerinos 4:1070977b83e9 28 void step(DigitalOut stepsPin,int voltage){
gardamerinos 4:1070977b83e9 29 stepsPin = voltage;
gardamerinos 4:1070977b83e9 30 }
simon 0:fb6bbc10ffa0 31
gardamerinos 5:9f6830d1db7b 32 void setDirections(vector<DigitalOut*> dirPin, int voltage[4]){
gardamerinos 5:9f6830d1db7b 33 for (int i = 0; i < 4; i++){
gardamerinos 5:9f6830d1db7b 34 *dirPin[i] = voltage[i];
gardamerinos 5:9f6830d1db7b 35 }
gardamerinos 5:9f6830d1db7b 36 }
gardamerinos 5:9f6830d1db7b 37
gardamerinos 5:9f6830d1db7b 38 void moveOneStep(){
gardamerinos 5:9f6830d1db7b 39 for (int i = 0; i<motors.size();i++){
gardamerinos 4:1070977b83e9 40 if (steps.at(i) == int(0)){
gardamerinos 4:1070977b83e9 41 motors.erase(motors.begin() + i);
gardamerinos 4:1070977b83e9 42 steps.erase(steps.begin()+i);
gardamerinos 4:1070977b83e9 43 }
gardamerinos 4:1070977b83e9 44 }
gardamerinos 4:1070977b83e9 45 for (int i = 0; i<motors.size();i++){
gardamerinos 4:1070977b83e9 46 step(*motors[i],1);
gardamerinos 4:1070977b83e9 47 }
gardamerinos 5:9f6830d1db7b 48 wait_us(delayus);
gardamerinos 4:1070977b83e9 49 for (int i = 0; i<motors.size();i++){
gardamerinos 4:1070977b83e9 50 step(*motors.at(i),0);
gardamerinos 5:9f6830d1db7b 51 steps.at(i) = steps.at(i)-1;
gardamerinos 4:1070977b83e9 52 }
gardamerinos 5:9f6830d1db7b 53 wait_us(delayusOffset);
gardamerinos 5:9f6830d1db7b 54 }
gardamerinos 5:9f6830d1db7b 55
gardamerinos 5:9f6830d1db7b 56 vector<string> splitString(string input){
gardamerinos 5:9f6830d1db7b 57 istringstream iss(input);
gardamerinos 5:9f6830d1db7b 58 vector<string> output;
gardamerinos 5:9f6830d1db7b 59 while (iss) {
gardamerinos 5:9f6830d1db7b 60 string word;
gardamerinos 5:9f6830d1db7b 61 iss >> word;
gardamerinos 5:9f6830d1db7b 62 output.push_back(word);
gardamerinos 5:9f6830d1db7b 63 }
gardamerinos 5:9f6830d1db7b 64 return output;
gardamerinos 5:9f6830d1db7b 65 }
gardamerinos 5:9f6830d1db7b 66
gardamerinos 5:9f6830d1db7b 67 void callback(){
gardamerinos 5:9f6830d1db7b 68 pc.scanf("%s", &msg);
gardamerinos 5:9f6830d1db7b 69 vector<string> stringPos= splitString(msg);
gardamerinos 5:9f6830d1db7b 70 goalPosition.clear();
gardamerinos 5:9f6830d1db7b 71 for (int i = 0; i<stringPos.size(); i++){
gardamerinos 5:9f6830d1db7b 72 goalPosition.push_back(atoi(stringPos[i].c_str()));
simon 0:fb6bbc10ffa0 73 }
simon 0:fb6bbc10ffa0 74 }
gardamerinos 5:9f6830d1db7b 75
gardamerinos 5:9f6830d1db7b 76 void initialization(){
gardamerinos 5:9f6830d1db7b 77 motors.push_back(new DigitalOut(p21));
gardamerinos 5:9f6830d1db7b 78 motors.push_back(new DigitalOut(p22));
gardamerinos 5:9f6830d1db7b 79 motors.push_back(new DigitalOut(p23));
gardamerinos 5:9f6830d1db7b 80 motors.push_back(new DigitalOut(p24));
gardamerinos 5:9f6830d1db7b 81 steps.push_back(stepsX);
gardamerinos 5:9f6830d1db7b 82 steps.push_back(stepsY);
gardamerinos 5:9f6830d1db7b 83 steps.push_back(stepsZ);
gardamerinos 5:9f6830d1db7b 84 steps.push_back(stepsRot);
gardamerinos 5:9f6830d1db7b 85 dirs.push_back(new DigitalOut(p30));
gardamerinos 5:9f6830d1db7b 86 dirs.push_back(new DigitalOut(p29));
gardamerinos 5:9f6830d1db7b 87 dirs.push_back(new DigitalOut(p28));
gardamerinos 5:9f6830d1db7b 88 dirs.push_back(new DigitalOut(p27));
gardamerinos 5:9f6830d1db7b 89 }
gardamerinos 5:9f6830d1db7b 90
gardamerinos 5:9f6830d1db7b 91 //=================MAIN========================================
gardamerinos 5:9f6830d1db7b 92 int main() {
gardamerinos 5:9f6830d1db7b 93 initialization();
gardamerinos 5:9f6830d1db7b 94 setDirections(dirs, positiveDirs);
gardamerinos 5:9f6830d1db7b 95 pc.attach(&callback);
gardamerinos 5:9f6830d1db7b 96
gardamerinos 5:9f6830d1db7b 97 while(1) {
gardamerinos 5:9f6830d1db7b 98 moveOneStep();
gardamerinos 5:9f6830d1db7b 99
gardamerinos 5:9f6830d1db7b 100 }
gardamerinos 5:9f6830d1db7b 101 }