Onishi Itsuki / Mbed 2 deprecated mbed_PWM_control

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

main.cpp@0:762583fdd0de, 2020-07-16 (annotated)

Committer:
onishiitsuki
Date:
Thu Jul 16 05:05:16 2020 +0000
Revision:
0:762583fdd0de
Child:
1:ccc6ed1e4cd8
share

Who changed what in which revision?

UserRevisionLine numberNew contents of line
onishiitsuki 0:762583fdd0de 1 // Construction macine controler for MBET
onishiitsuki 0:762583fdd0de 2 // Onishi Itsuki
onishiitsuki 0:762583fdd0de 3 // July 2020
onishiitsuki 0:762583fdd0de 4
onishiitsuki 0:762583fdd0de 5 #include "mbed.h"
onishiitsuki 0:762583fdd0de 6
onishiitsuki 0:762583fdd0de 7 #define START_BYTE 0x0f
onishiitsuki 0:762583fdd0de 8 #define BYTE_A 2*i
onishiitsuki 0:762583fdd0de 9 #define BYTE_B 2*i+1
onishiitsuki 0:762583fdd0de 10
onishiitsuki 0:762583fdd0de 11 uint8_t buffer[8];
onishiitsuki 0:762583fdd0de 12 int outputBuffer[12];
onishiitsuki 0:762583fdd0de 13 int bufferIndex = 0;
onishiitsuki 0:762583fdd0de 14 int outProcCounter = 0;
onishiitsuki 0:762583fdd0de 15 int outProcFlag = 0;
onishiitsuki 0:762583fdd0de 16
onishiitsuki 0:762583fdd0de 17 Ticker ticker;
onishiitsuki 0:762583fdd0de 18 Serial pc(USBTX, USBRX);
onishiitsuki 0:762583fdd0de 19 DigitalOut stateChecker(LED1);
onishiitsuki 0:762583fdd0de 20 DigitalOut startByteError(LED3);
onishiitsuki 0:762583fdd0de 21 DigitalOut parityByteError(LED4);
onishiitsuki 0:762583fdd0de 22
onishiitsuki 0:762583fdd0de 23 // set output pins
onishiitsuki 0:762583fdd0de 24 DigitalOut boomOutA(p21);
onishiitsuki 0:762583fdd0de 25 DigitalOut boomOutB(p22);
onishiitsuki 0:762583fdd0de 26 DigitalOut armOutA(p23);
onishiitsuki 0:762583fdd0de 27 DigitalOut armOutB(p24);
onishiitsuki 0:762583fdd0de 28 DigitalOut bucketOutA(p25);
onishiitsuki 0:762583fdd0de 29 DigitalOut bucketOutB(p26);
onishiitsuki 0:762583fdd0de 30 DigitalOut slewingOutA(p20);
onishiitsuki 0:762583fdd0de 31 DigitalOut slewingOutB(p19);
onishiitsuki 0:762583fdd0de 32 DigitalOut rightWheelOutA(p18);
onishiitsuki 0:762583fdd0de 33 DigitalOut rightWheelOutB(p17);
onishiitsuki 0:762583fdd0de 34 DigitalOut leftWheelOutA(p16);
onishiitsuki 0:762583fdd0de 35 DigitalOut leftWheelOutB(p15);
onishiitsuki 0:762583fdd0de 36
onishiitsuki 0:762583fdd0de 37
onishiitsuki 0:762583fdd0de 38 void set_test_buffer(void)
onishiitsuki 0:762583fdd0de 39 {
onishiitsuki 0:762583fdd0de 40 buffer[0] = 31;
onishiitsuki 0:762583fdd0de 41 buffer[1] = 250;
onishiitsuki 0:762583fdd0de 42 buffer[2] = 200;
onishiitsuki 0:762583fdd0de 43 buffer[3] = 150;
onishiitsuki 0:762583fdd0de 44 buffer[4] = 100;
onishiitsuki 0:762583fdd0de 45 buffer[5] = 50;
onishiitsuki 0:762583fdd0de 46 buffer[6] = 10;
onishiitsuki 0:762583fdd0de 47 buffer[7] = 248;
onishiitsuki 0:762583fdd0de 48 }
onishiitsuki 0:762583fdd0de 49
onishiitsuki 0:762583fdd0de 50
onishiitsuki 0:762583fdd0de 51 void set_output(void)
onishiitsuki 0:762583fdd0de 52 {
onishiitsuki 0:762583fdd0de 53 boomOutA = outputBuffer[0];
onishiitsuki 0:762583fdd0de 54 boomOutB = outputBuffer[1];
onishiitsuki 0:762583fdd0de 55 armOutA = outputBuffer[2];
onishiitsuki 0:762583fdd0de 56 armOutB = outputBuffer[3];
onishiitsuki 0:762583fdd0de 57 bucketOutA = outputBuffer[4];
onishiitsuki 0:762583fdd0de 58 bucketOutB = outputBuffer[5];
onishiitsuki 0:762583fdd0de 59 slewingOutA = outputBuffer[6];
onishiitsuki 0:762583fdd0de 60 slewingOutB = outputBuffer[7];
onishiitsuki 0:762583fdd0de 61 rightWheelOutA = outputBuffer[8];
onishiitsuki 0:762583fdd0de 62 rightWheelOutB = outputBuffer[9];
onishiitsuki 0:762583fdd0de 63 leftWheelOutA = outputBuffer[10];
onishiitsuki 0:762583fdd0de 64 leftWheelOutB = outputBuffer[11];
onishiitsuki 0:762583fdd0de 65 }
onishiitsuki 0:762583fdd0de 66
onishiitsuki 0:762583fdd0de 67
onishiitsuki 0:762583fdd0de 68 void init_state(void)
onishiitsuki 0:762583fdd0de 69 {
onishiitsuki 0:762583fdd0de 70 for(int i=0; i<6; i++) {
onishiitsuki 0:762583fdd0de 71 outputBuffer[BYTE_A] = 0;
onishiitsuki 0:762583fdd0de 72 outputBuffer[BYTE_B] = 0;
onishiitsuki 0:762583fdd0de 73 }
onishiitsuki 0:762583fdd0de 74 set_output();
onishiitsuki 0:762583fdd0de 75 }
onishiitsuki 0:762583fdd0de 76
onishiitsuki 0:762583fdd0de 77
onishiitsuki 0:762583fdd0de 78 void PWM_processor(void)
onishiitsuki 0:762583fdd0de 79 {
onishiitsuki 0:762583fdd0de 80 for(int i=0; i<6; i++) {
onishiitsuki 0:762583fdd0de 81 if((buffer[i+1]==0) || (buffer[i+1]==128)) {
onishiitsuki 0:762583fdd0de 82 outputBuffer[BYTE_A] = 0;
onishiitsuki 0:762583fdd0de 83 outputBuffer[BYTE_B] = 0;
onishiitsuki 0:762583fdd0de 84 continue;
onishiitsuki 0:762583fdd0de 85 }
onishiitsuki 0:762583fdd0de 86
onishiitsuki 0:762583fdd0de 87 if(buffer[i+1]>128) {
onishiitsuki 0:762583fdd0de 88 outputBuffer[BYTE_A] = 0;
onishiitsuki 0:762583fdd0de 89 outputBuffer[BYTE_B] = 1;
onishiitsuki 0:762583fdd0de 90 } else {
onishiitsuki 0:762583fdd0de 91 outputBuffer[BYTE_A] = 1;
onishiitsuki 0:762583fdd0de 92 outputBuffer[BYTE_B] = 0;
onishiitsuki 0:762583fdd0de 93 }
onishiitsuki 0:762583fdd0de 94
onishiitsuki 0:762583fdd0de 95 // decrement
onishiitsuki 0:762583fdd0de 96 if (buffer[i+1] > 0)
onishiitsuki 0:762583fdd0de 97 buffer[i+1]--;
onishiitsuki 0:762583fdd0de 98 }
onishiitsuki 0:762583fdd0de 99 set_output();
onishiitsuki 0:762583fdd0de 100 }
onishiitsuki 0:762583fdd0de 101
onishiitsuki 0:762583fdd0de 102
onishiitsuki 0:762583fdd0de 103 void event_func(void)
onishiitsuki 0:762583fdd0de 104 {
onishiitsuki 0:762583fdd0de 105 stateChecker = 1;
onishiitsuki 0:762583fdd0de 106
onishiitsuki 0:762583fdd0de 107 // For test
onishiitsuki 0:762583fdd0de 108 bufferIndex = 8;
onishiitsuki 0:762583fdd0de 109 set_test_buffer();
onishiitsuki 0:762583fdd0de 110 // for(int i=0; i<8; i++){
onishiitsuki 0:762583fdd0de 111 // pc.printf("%d:%d ", i, buffer[i]);
onishiitsuki 0:762583fdd0de 112 // }
onishiitsuki 0:762583fdd0de 113 uint8_t rx = buffer[0];
onishiitsuki 0:762583fdd0de 114 // end of code for test
onishiitsuki 0:762583fdd0de 115
onishiitsuki 0:762583fdd0de 116 // uint8_t rx = pc.getc();
onishiitsuki 0:762583fdd0de 117 //
onishiitsuki 0:762583fdd0de 118 // wait start byte
onishiitsuki 0:762583fdd0de 119 if(bufferIndex == 0 && rx != START_BYTE) {
onishiitsuki 0:762583fdd0de 120 startByteError = 1;
onishiitsuki 0:762583fdd0de 121 pc.printf("start byte ERROR");
onishiitsuki 0:762583fdd0de 122 return;
onishiitsuki 0:762583fdd0de 123 } else {
onishiitsuki 0:762583fdd0de 124 startByteError = 0;
onishiitsuki 0:762583fdd0de 125 }
onishiitsuki 0:762583fdd0de 126
onishiitsuki 0:762583fdd0de 127 // // push serial to buffer
onishiitsuki 0:762583fdd0de 128 // buffer[bufferIndex++] = rx;
onishiitsuki 0:762583fdd0de 129
onishiitsuki 0:762583fdd0de 130 // when buffer is full
onishiitsuki 0:762583fdd0de 131 if(bufferIndex == 8) {
onishiitsuki 0:762583fdd0de 132 bufferIndex = 0;
onishiitsuki 0:762583fdd0de 133
onishiitsuki 0:762583fdd0de 134 // check parity
onishiitsuki 0:762583fdd0de 135 // uint8_t serialXOR = buffer[1];
onishiitsuki 0:762583fdd0de 136 // for (int i=2; i<=6; i++) {
onishiitsuki 0:762583fdd0de 137 // serialXOR ^= buffer[i];
onishiitsuki 0:762583fdd0de 138 // }
onishiitsuki 0:762583fdd0de 139 // if (serialXOR != buffer[7]) {
onishiitsuki 0:762583fdd0de 140 // parityByteError = 1;
onishiitsuki 0:762583fdd0de 141 // pc.printf("parity byte ERROR: XOR is %d, buffer byte is %d ", serialXOR, buffer[7]);
onishiitsuki 0:762583fdd0de 142 // return;
onishiitsuki 0:762583fdd0de 143 // } else {
onishiitsuki 0:762583fdd0de 144 // outProcFlag = 1;
onishiitsuki 0:762583fdd0de 145 // parityByteError = 0;
onishiitsuki 0:762583fdd0de 146 // }
onishiitsuki 0:762583fdd0de 147 }
onishiitsuki 0:762583fdd0de 148 }
onishiitsuki 0:762583fdd0de 149
onishiitsuki 0:762583fdd0de 150 int main(void)
onishiitsuki 0:762583fdd0de 151 {
onishiitsuki 0:762583fdd0de 152 pc.baud(921600);
onishiitsuki 0:762583fdd0de 153 startByteError = 0;
onishiitsuki 0:762583fdd0de 154 parityByteError = 0;
onishiitsuki 0:762583fdd0de 155 int test_count = 0;
onishiitsuki 0:762583fdd0de 156
onishiitsuki 0:762583fdd0de 157 // initial state is free run
onishiitsuki 0:762583fdd0de 158 init_state();
onishiitsuki 0:762583fdd0de 159 wait(1);
onishiitsuki 0:762583fdd0de 160 ticker.attach_us(PWM_processor, 500.0f);
onishiitsuki 0:762583fdd0de 161 pc.printf("+");
onishiitsuki 0:762583fdd0de 162
onishiitsuki 0:762583fdd0de 163 while(1) {
onishiitsuki 0:762583fdd0de 164 stateChecker = 0;
onishiitsuki 0:762583fdd0de 165 // if(pc.readable(){
onishiitsuki 0:762583fdd0de 166 event_func();
onishiitsuki 0:762583fdd0de 167 test_count++;
onishiitsuki 0:762583fdd0de 168 // }
onishiitsuki 0:762583fdd0de 169
onishiitsuki 0:762583fdd0de 170 // code for test
onishiitsuki 0:762583fdd0de 171 if(test_count%100000 == 0) {
onishiitsuki 0:762583fdd0de 172 pc.printf("+");
onishiitsuki 0:762583fdd0de 173 test_count = 1;
onishiitsuki 0:762583fdd0de 174 }
onishiitsuki 0:762583fdd0de 175 // end of code for test
onishiitsuki 0:762583fdd0de 176
onishiitsuki 0:762583fdd0de 177 }
onishiitsuki 0:762583fdd0de 178 }