Shih-Ho Hsieh / Mbed 2 deprecated pololu5mag_with_platform

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of Motor_XYZ_UI_SPI_I2C_5mag by Shih-Ho Hsieh

ui.cpp@11:bb86ffb378f4, 2018-01-05 (annotated)

Committer:
hober
Date:
Fri Jan 05 08:06:13 2018 +0000
Branch:
envelope
Revision:
11:bb86ffb378f4
Parent:
10:a49cdab3810f
Child:
12:2e1b1b1726fa
update fs 20180105

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hober 6:ce02d396c961 1 #include "motor.h"
hober 6:ce02d396c961 2 #include "xyz_sensor_platform.h"
hober 8:33d34a775873 3 //#include "ParseArray.h"
hober 8:33d34a775873 4 #include "envelopetracker.h"
hober 6:ce02d396c961 5
hober 7:ee0569d49c52 6 #define I2C_FREQUENCY 400000
hober 6:ce02d396c961 7
hober 8:33d34a775873 8 typedef unsigned char byte;
hober 10:a49cdab3810f 9 uint8_t* dataToSend;
hober 8:33d34a775873 10 int sendArrayIndex = 0;
hober 8:33d34a775873 11 int sendBufferMax = 10000;
hober 8:33d34a775873 12 const int BAUD = 921600;
hober 8:33d34a775873 13 Serial pc(SERIAL_TX, SERIAL_RX, BAUD );
hober 7:ee0569d49c52 14 DigitalOut led(LED2),led3(LED3);
hober 7:ee0569d49c52 15 DigitalOut mag_test(D11);
hober 6:ce02d396c961 16 XYZSensorPlatform platform;
hober 8:33d34a775873 17 Envelope *command;
hober 8:33d34a775873 18 EnvelopeTracker tracker;
hober 7:ee0569d49c52 19 byte commandToSend[10]= {'H','O','1','2','3','4','5','6','E',0};
hober 7:ee0569d49c52 20 bool isRecording = false;
hober 9:c4f7257dee47 21 Envelope* result;
hober 9:c4f7257dee47 22 float x, y, z;
hober 9:c4f7257dee47 23 float pos[3];
hober 9:c4f7257dee47 24 int n = 0;
hober 9:c4f7257dee47 25 int getMag = 0;
hober 9:c4f7257dee47 26 int leftCount = 0;
hober 9:c4f7257dee47 27 int rightCount = 0;
hober 9:c4f7257dee47 28 int upCount = 0;
hober 9:c4f7257dee47 29 int downCount = 0;
hober 9:c4f7257dee47 30 int forwardCount = 0;
hober 9:c4f7257dee47 31 int backwardCount = 0;
hober 9:c4f7257dee47 32 bool commandToDo = false;
hober 9:c4f7257dee47 33 int recordTime;
hober 9:c4f7257dee47 34 float waitTime;
hober 8:33d34a775873 35
hober 9:c4f7257dee47 36 const int Fs = 1000; // sampling rate -- max: 1kHz
hober 8:33d34a775873 37
hober 8:33d34a775873 38
hober 6:ce02d396c961 39 void echo(char typ, float x, float y, float z);
hober 6:ce02d396c961 40 void echo(char typ, int16_t *p_data);
hober 9:c4f7257dee47 41 void Rx_interrupt();
hober 8:33d34a775873 42
hober 6:ce02d396c961 43 int main()
hober 6:ce02d396c961 44 {
hober 6:ce02d396c961 45 led=1;
hober 8:33d34a775873 46 command = new Envelope;
hober 8:33d34a775873 47 command->enableHeader(std::string("H"));// 48 H
hober 8:33d34a775873 48 command->enableFooter(std::string("E"),8);// 45 E
hober 8:33d34a775873 49 command->enableCheckXOR(9);
hober 8:33d34a775873 50 tracker.setEnvelope(*command);
hober 8:33d34a775873 51 tracker.setBufferLength(100);
hober 7:ee0569d49c52 52 pc.format(8,SerialBase::None,1);
hober 6:ce02d396c961 53 platform.set_speed(2.5);
hober 7:ee0569d49c52 54 platform.reset(); // need to be modified here
hober 7:ee0569d49c52 55 platform.setSensorI2cFrequency(I2C_FREQUENCY);
hober 9:c4f7257dee47 56 // Setup a serial interrupt function to receive data
hober 9:c4f7257dee47 57 pc.attach(&Rx_interrupt, Serial::RxIrq);
hober 6:ce02d396c961 58 while(1) {
hober 8:33d34a775873 59 if(isRecording && n < recordTime) {
hober 7:ee0569d49c52 60 int16_t mag[3];
hober 7:ee0569d49c52 61 if(platform.get_mag_raw(mag)==0&&pc.writeable()) {
hober 7:ee0569d49c52 62 echo('M',mag);
hober 7:ee0569d49c52 63 mag_test=!mag_test;
hober 8:33d34a775873 64 wait(waitTime);
hober 7:ee0569d49c52 65 }
hober 7:ee0569d49c52 66 n++;
hober 8:33d34a775873 67 } else if(isRecording) {
hober 8:33d34a775873 68 isRecording = false;
hober 8:33d34a775873 69 echo('S',0,0,0);
hober 8:33d34a775873 70 }// end recording if
hober 9:c4f7257dee47 71 if(getMag>0) {
hober 9:c4f7257dee47 72 int16_t mag[3];
hober 9:c4f7257dee47 73 if(platform.get_mag_raw(mag)==0) echo('M',mag);
hober 9:c4f7257dee47 74 getMag--;
hober 9:c4f7257dee47 75 }
hober 9:c4f7257dee47 76 if(commandToDo) {
hober 9:c4f7257dee47 77 platform.to(x,y,z);
hober 9:c4f7257dee47 78 platform.position(pos);
hober 9:c4f7257dee47 79 echo('O',pos[0],pos[1],pos[2]);
hober 9:c4f7257dee47 80 commandToDo = false;
hober 9:c4f7257dee47 81 }
hober 9:c4f7257dee47 82 if(leftCount > 0||rightCount > 0||upCount > 0||downCount > 0||forwardCount > 0||backwardCount > 0) {
hober 9:c4f7257dee47 83 if(leftCount > 0) {
hober 9:c4f7257dee47 84 platform.go_left();
hober 9:c4f7257dee47 85 leftCount--;
hober 9:c4f7257dee47 86 }
hober 9:c4f7257dee47 87 if(rightCount > 0) {
hober 9:c4f7257dee47 88 platform.go_right();
hober 9:c4f7257dee47 89 rightCount--;
hober 9:c4f7257dee47 90 }
hober 9:c4f7257dee47 91 if(upCount > 0) {
hober 9:c4f7257dee47 92 platform.go_up();
hober 9:c4f7257dee47 93 upCount--;
hober 9:c4f7257dee47 94 }
hober 9:c4f7257dee47 95 if(downCount > 0) {
hober 9:c4f7257dee47 96 platform.go_down();
hober 9:c4f7257dee47 97 downCount--;
hober 9:c4f7257dee47 98 }
hober 9:c4f7257dee47 99 if(forwardCount > 0) {
hober 9:c4f7257dee47 100 platform.go_forward();
hober 9:c4f7257dee47 101 forwardCount--;
hober 9:c4f7257dee47 102 }
hober 9:c4f7257dee47 103 if(backwardCount > 0) {
hober 9:c4f7257dee47 104 platform.go_backward();
hober 9:c4f7257dee47 105 backwardCount--;
hober 9:c4f7257dee47 106 }
hober 9:c4f7257dee47 107 platform.position(pos);
hober 9:c4f7257dee47 108 echo('O',pos[0],pos[1],pos[2]);
hober 9:c4f7257dee47 109 }
hober 7:ee0569d49c52 110 } // end while
hober 6:ce02d396c961 111
hober 6:ce02d396c961 112 }
hober 6:ce02d396c961 113
hober 6:ce02d396c961 114 void echo(char typ,float x, float y, float z)
hober 6:ce02d396c961 115 {
hober 7:ee0569d49c52 116 int16_t p_data[3]= {(int16_t)(x*10), int16_t(y*10), int16_t(z*10)};
hober 7:ee0569d49c52 117 echo(typ,p_data);
hober 6:ce02d396c961 118 }
hober 6:ce02d396c961 119
hober 6:ce02d396c961 120 void echo(char typ, int16_t *p_data)
hober 6:ce02d396c961 121 {
hober 8:33d34a775873 122 char tmp[] = {typ, p_data[0]>>8, p_data[0], p_data[1]>>8, p_data[1], p_data[2]>>8, p_data[2]};
hober 8:33d34a775873 123 command->setEnvelopeData(tmp,7);
hober 10:a49cdab3810f 124 dataToSend = (uint8_t*)(command->getEnvelopeArray());
hober 8:33d34a775873 125 for(int i = 0; i < command->length(); i++) {
hober 10:a49cdab3810f 126 pc.putc(dataToSend[i]);
hober 8:33d34a775873 127 }
hober 6:ce02d396c961 128 }
hober 6:ce02d396c961 129
hober 9:c4f7257dee47 130 void Rx_interrupt()
hober 9:c4f7257dee47 131 {
hober 9:c4f7257dee47 132 char c;
hober 9:c4f7257dee47 133 while(pc.readable()) {
hober 9:c4f7257dee47 134 c = pc.getc();
hober 9:c4f7257dee47 135 tracker.parse(&c,1);
hober 6:ce02d396c961 136
hober 9:c4f7257dee47 137 result = tracker.getEnvelope();
hober 9:c4f7257dee47 138 if(result!=NULL) {
hober 9:c4f7257dee47 139 char *dataArray = result->getPayload();
hober 9:c4f7257dee47 140 switch(dataArray[0]) {
hober 9:c4f7257dee47 141 case 'I':
hober 9:c4f7257dee47 142 break;
hober 9:c4f7257dee47 143 case 'O': // echo
hober 9:c4f7257dee47 144 platform.position(pos);
hober 9:c4f7257dee47 145 echo('O',pos[0],pos[1],pos[2]);
hober 9:c4f7257dee47 146 led3 = !led3;
hober 9:c4f7257dee47 147 break;
hober 9:c4f7257dee47 148 case 'C': // command
hober 9:c4f7257dee47 149 x=(float)((dataArray[1]<<8)+dataArray[2])/10.0f;
hober 9:c4f7257dee47 150 y=(float)((dataArray[3]<<8)+dataArray[4])/10.0f;
hober 9:c4f7257dee47 151 z=(float)((dataArray[5]<<8)+dataArray[6])/10.0f;
hober 9:c4f7257dee47 152 commandToDo = true;
hober 9:c4f7257dee47 153 break;
hober 9:c4f7257dee47 154 case 'X':
hober 9:c4f7257dee47 155 if(dataArray[1]&0x80) rightCount++;
hober 9:c4f7257dee47 156 else leftCount++;
hober 9:c4f7257dee47 157 break;
hober 9:c4f7257dee47 158 case 'Y':
hober 9:c4f7257dee47 159 if(dataArray[1]&0x80) forwardCount++;
hober 10:a49cdab3810f 160 else backwardCount++;
hober 9:c4f7257dee47 161 break;
hober 9:c4f7257dee47 162 case 'Z':
hober 9:c4f7257dee47 163 if(dataArray[1]&0x80) upCount++;
hober 9:c4f7257dee47 164 else downCount++;
hober 9:c4f7257dee47 165 break;
hober 9:c4f7257dee47 166 case 'M': // magnet
hober 9:c4f7257dee47 167 getMag++;
hober 9:c4f7257dee47 168 mag_test=!mag_test;
hober 9:c4f7257dee47 169 break;
hober 9:c4f7257dee47 170 case 'R': // record
hober 9:c4f7257dee47 171 recordTime = dataArray[1];
hober 9:c4f7257dee47 172 recordTime *= Fs;
hober 11:bb86ffb378f4 173 waitTime = 1.0/Fs-0.00052-1/(BAUD/8/10);
hober 10:a49cdab3810f 174 n = 0;
hober 9:c4f7257dee47 175 if(waitTime < 0) waitTime = 0;
hober 9:c4f7257dee47 176 isRecording = true;
hober 9:c4f7257dee47 177 break;
hober 9:c4f7257dee47 178 case 'S': // stop
hober 9:c4f7257dee47 179 isRecording = false;
hober 9:c4f7257dee47 180 break;
hober 9:c4f7257dee47 181 default:
hober 9:c4f7257dee47 182 break;
hober 9:c4f7257dee47 183 } // end switch
hober 10:a49cdab3810f 184 // delete dataArray;
hober 9:c4f7257dee47 185 result = NULL;
hober 9:c4f7257dee47 186 dataArray = NULL;
hober 9:c4f7257dee47 187 } // end result if
hober 9:c4f7257dee47 188 } // end parsing if
hober 9:c4f7257dee47 189 }
hober 9:c4f7257dee47 190